Some tweaks to wp, basically works

zone/client_packet.cpp

@@ -4366,6 +4366,7 @@ void Client::Handle_OP_ClientTimeStamp(const EQApplicationPacket *app)
 
 void Client::Handle_OP_ClientUpdate(const EQApplicationPacket *app)
 {
+	Log(Logs::General, Logs::Status, "Incoming client position packet");
 	if (IsAIControlled())
 		return;
 
@@ -4606,6 +4607,7 @@ void Client::Handle_OP_ClientUpdate(const EQApplicationPacket *app)
 	m_Position.x = ppu->x_pos;
 	m_Position.y = ppu->y_pos;
 	m_Position.z = ppu->z_pos;
+	Log(Logs::General, Logs::Status, "Client position updated");
 	
 	/* Visual Debugging */
 	if (RuleB(Character, OPClientUpdateVisualDebug)) {

zone/mob.cpp

@@ -444,6 +444,8 @@ Mob::Mob(const char* in_name,
 	PrimaryAggro = false;
 	AssistAggro = false;
 	npc_assist_cap = 0;
+
+	PathRecalcTimer.reset(new Timer(1000));
 }
 
 Mob::~Mob()

zone/mob.h

@@ -1410,6 +1410,7 @@ protected:
 	//
 	glm::vec3 PathingDestination;
 	IPathfinder::IPath Route;
+	std::unique_ptr<Timer> PathRecalcTimer;
 	bool DistractedFromGrid;
 
 	uint32 pDontHealMeBefore;

zone/pathfinder_waypoint.cpp

@@ -46,7 +46,8 @@ struct Edge
 	float distance;
 	bool teleport;
 	int door_id;
-};
+};
+
 template <class Graph, class CostType, class NodeMap>
 class distance_heuristic : public boost::astar_heuristic<Graph, CostType>
 {
@@ -94,7 +95,7 @@ struct PathfinderWaypoint::Implementation {
 	boost::geometry::index::rtree<RTreeValue, boost::geometry::index::quadratic<16>> Tree;
 	GraphType Graph;
 	std::vector<Node> Nodes;
-	std::vector<Edge> Edges;
+	std::map<std::pair<size_t, size_t>, Edge> Edges;
 };
 
 PathfinderWaypoint::PathfinderWaypoint(const std::string &path)
@@ -171,7 +172,8 @@ PathfinderWaypoint::PathfinderWaypoint(const std::string &path)
 					edge.distance = PathNodes[i].Neighbours[j].distance;
 					edge.door_id = PathNodes[i].Neighbours[j].DoorID;
 					edge.teleport = PathNodes[i].Neighbours[j].Teleport;
-					m_impl->Edges.push_back(edge);
+
+					m_impl->Edges[std::make_pair(PathNodes[i].id, PathNodes[i].Neighbours[j].id)] = edge;
 				}
 			}
 		}
@@ -207,16 +209,18 @@ IPathfinder::IPath PathfinderWaypoint::FindRoute(const glm::vec3 &start, const g
 	auto &nearest_start = *result_start_n.begin();
 	auto &nearest_end = *result_end_n.begin();
 	
-	Log(Logs::General, Logs::Status, "Nearest start point found to be (%f, %f, %f) with node %u", nearest_start.first.get<0>(), nearest_start.first.get<1>(), nearest_start.first.get<2>(), nearest_start.second);
-	Log(Logs::General, Logs::Status, "Nearest end point found to be (%f, %f, %f) with node %u", nearest_end.first.get<0>(), nearest_end.first.get<1>(), nearest_end.first.get<2>(), nearest_end.second);
+	if (nearest_start.second == nearest_end.second) {
+		IPath Route;
+		Route.push_back(start);
+		Route.push_back(end);
+		return Route;
+	}
 
 	std::vector<GraphType::vertex_descriptor> p(boost::num_vertices(m_impl->Graph));
-	std::vector<float> d(boost::num_vertices(m_impl->Graph));
 	try {
 		boost::astar_search(m_impl->Graph, nearest_start.second, 
 			distance_heuristic<GraphType, float, Node*>(&m_impl->Nodes[0], nearest_end.second),
 			boost::predecessor_map(&p[0])
-				.distance_map(&d[0])
 				.visitor(astar_goal_visitor<size_t>(nearest_end.second)));
 	}
 	catch (found_goal)
@@ -225,16 +229,33 @@ IPathfinder::IPath PathfinderWaypoint::FindRoute(const glm::vec3 &start, const g
 		
 		Route.push_front(end);
 		for (size_t v = nearest_end.second;; v = p[v]) {
-			Route.push_front(m_impl->Nodes[v].v);
-			if (p[v] == v)
+			if (p[v] == v) {
+				Route.push_front(m_impl->Nodes[v].v);
 				break;
+			}
+			else {
+				auto iter = m_impl->Edges.find(std::make_pair(p[v], p[v + 1]));
+				if (iter != m_impl->Edges.end()) {
+					auto &edge = iter->second;
+					if (edge.teleport) {
+						Route.push_front(m_impl->Nodes[v].v);
+						glm::vec3 teleport(100000000.0f, 100000000.0f, 100000000.0f);
+						Route.push_front(teleport);
+					}
+					else {
+						Route.push_front(m_impl->Nodes[v].v);
+					}
+				}
+				else {
+					Route.push_front(m_impl->Nodes[v].v);
+				}
+			}
 		}
 		Route.push_front(start);
 
 		return Route;
 	}
 
-
 	IPath Route;
 	Route.push_back(start);
 	Route.push_back(end);

zone/pathing.cpp

@@ -666,61 +666,62 @@ glm::vec3 Mob::UpdatePath(float ToX, float ToY, float ToZ, float Speed, bool &Wa
 		return *Route.begin();
 	}
 	else {
-		bool SameDestination = DistanceSquared(To, PathingDestination) < 4.0f;
-		if (!SameDestination) {
-			//We had a route but our target position moved too much
-			Route = zone->pathing->FindRoute(From, To);
-			PathingDestination = To;
-			WaypointChanged = true;
-			NodeReached = false;
-			return *Route.begin();
-		}
-		else {
-			bool AtNextNode = DistanceSquared(From, *Route.begin()) < 4.0f;
-			if (AtNextNode) {
-				WaypointChanged = false;
-				NodeReached = true;
-
-				Route.pop_front();
-
-				if (Route.empty()) {
-					Route = zone->pathing->FindRoute(From, To);
-					PathingDestination = To;
-					WaypointChanged = true;
-					return *Route.begin();
-				}
-				else {
-					auto node = *Route.begin();
-					if (node.x == 1000000.0f && node.y == 1000000.0f && node.z == 1000000.0f) {
-						//If is identity node then is teleport node.
-						Route.pop_front();
-
-						if (Route.empty()) {
-							return To;
-						}
-
-						auto nextNode = *Route.begin();
-
-						Teleport(nextNode);
-
-						Route.pop_front();
-
-						if (Route.empty()) {
-							return To;
-						}
-
-						return *Route.begin();
-					}
-
-					return node;
-				}
-			}
-			else {
-				WaypointChanged = false;
+		if (PathRecalcTimer->Check()) {
+			bool SameDestination = DistanceSquared(To, PathingDestination) < 4.0f;
+			if (!SameDestination) {
+				//We had a route but our target position moved too much
+				Route = zone->pathing->FindRoute(From, To);
+				PathingDestination = To;
+				WaypointChanged = true;
 				NodeReached = false;
 				return *Route.begin();
 			}
 		}
+
+		bool AtNextNode = DistanceSquared(From, *Route.begin()) < 4.0f;
+		if (AtNextNode) {
+			WaypointChanged = false;
+			NodeReached = true;
+		
+			Route.pop_front();
+		
+			if (Route.empty()) {
+				Route = zone->pathing->FindRoute(From, To);
+				PathingDestination = To;
+				WaypointChanged = true;
+				return *Route.begin();
+			}
+			else {
+				auto node = *Route.begin();
+				if (node.x == 1000000.0f && node.y == 1000000.0f && node.z == 1000000.0f) {
+					//If is identity node then is teleport node.
+					Route.pop_front();
+		
+					if (Route.empty()) {
+						return To;
+					}
+		
+					auto nextNode = *Route.begin();
+		
+					Teleport(nextNode);
+		
+					Route.pop_front();
+		
+					if (Route.empty()) {
+						return To;
+					}
+		
+					return *Route.begin();
+				}
+		
+				return node;
+			}
+		}
+		else {
+			WaypointChanged = false;
+			NodeReached = false;
+			return *Route.begin();
+		}
 	}
 }