From 0dd4f74bee37e5d96d3205b5845515593be96a62 Mon Sep 17 00:00:00 2001
From: aozhiwei <aozhiwei@kingsome.cn>
Date: Mon, 10 Aug 2020 19:22:52 +0800
Subject: [PATCH] =?UTF-8?q?=E5=AE=8C=E6=88=90navmesh=E5=AF=BB=E8=B7=AF?=
 =?UTF-8?q?=E5=9F=BA=E7=A1=80?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 server/gameserver/mapservice.cc | 43 +++++++++++++++++++++++++++++++--
 1 file changed, 41 insertions(+), 2 deletions(-)

diff --git a/server/gameserver/mapservice.cc b/server/gameserver/mapservice.cc
index 24b1216..871be1c 100644
--- a/server/gameserver/mapservice.cc
+++ b/server/gameserver/mapservice.cc
@@ -15,6 +15,8 @@
 #include "room.h"
 #include "roomobstacle.h"
 
+static const int INVALID_NAVMESH_POLYREF = 0;
+
 MapService::MapService()
 {
     INIT_LIST_HEAD(&findpath_list_);
@@ -360,7 +362,7 @@ void MapService::FindPathUpdate(FindPathStatus* find_status)
 	filter.setExcludeFlags(0);
 
 	const float extents[3] = {2.f, 4.f, 2.f};
-#if 0
+
 	dtPolyRef startRef = INVALID_NAVMESH_POLYREF;
 	dtPolyRef endRef = INVALID_NAVMESH_POLYREF;
 
@@ -368,7 +370,44 @@ void MapService::FindPathUpdate(FindPathStatus* find_status)
 	float endNearestPt[3];
 	find_status->navmesh_query->findNearestPoly(spos, extents, &filter, &startRef, startNearestPt);
 	find_status->navmesh_query->findNearestPoly(epos, extents, &filter, &endRef, endNearestPt);
-#endif
+	if (!startRef || !endRef) {
+            find_status->navmesh_search_state = kNavMesh_SearchFailed;
+            return;
+        }
+        const int MAX_POLYS = 256;
+	dtPolyRef polys[MAX_POLYS];
+	int npolys;
+	float straightPath[MAX_POLYS * 3];
+	unsigned char straightPathFlags[MAX_POLYS];
+	dtPolyRef straightPathPolys[MAX_POLYS];
+	int nstraightPath;
+	int pos = 0;
+
+	find_status->navmesh_query->findPath(startRef, endRef, startNearestPt, endNearestPt, &filter, polys, &npolys, MAX_POLYS);
+	nstraightPath = 0;
+
+	if (npolys) {
+            float epos1[3];
+            dtVcopy(epos1, endNearestPt);
+
+            if (polys[npolys-1] != endRef) {
+                find_status->navmesh_query->closestPointOnPoly(polys[npolys-1], endNearestPt, epos1, 0);
+            }
+
+            find_status->navmesh_query->findStraightPath(startNearestPt, endNearestPt, polys, npolys, straightPath, straightPathFlags, straightPathPolys, &nstraightPath, MAX_POLYS);
+
+            #if 0
+            a8::Vec3 currpos;
+            for(int i = 0; i < nstraightPath * 3; ) {
+                currpos.x = straightPath[i++];
+                currpos.y = straightPath[i++];
+                currpos.z = straightPath[i++];
+                paths.push_back(currpos);
+                pos++;
+            }
+            #endif
+	}
+
     } else {
         do {
             find_status->search_state = find_status->astar_search.SearchStep();