From 686ab9de3a30d9c3232b5adae1449776b4edb53d Mon Sep 17 00:00:00 2001
From: kercylan98 <kercylan@gmail.com>
Date: Mon, 19 Jun 2023 20:17:40 +0800
Subject: [PATCH] =?UTF-8?q?:sparkles:=20=E7=BD=91=E6=A0=BC=E5=AF=BB?=
 =?UTF-8?q?=E8=B7=AF=20NavMesh=20=E5=AE=9E=E7=8E=B0?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 utils/geometry/navmesh/funnel.go  | 83 +++++++++++++++++++++++++++++++
 utils/geometry/navmesh/navmesh.go | 35 ++++++++++++-
 utils/geometry/navmesh/shape.go   |  4 +-
 3 files changed, 120 insertions(+), 2 deletions(-)
 create mode 100644 utils/geometry/navmesh/funnel.go

diff --git a/utils/geometry/navmesh/funnel.go b/utils/geometry/navmesh/funnel.go
new file mode 100644
index 0000000..4755e5c
--- /dev/null
+++ b/utils/geometry/navmesh/funnel.go
@@ -0,0 +1,83 @@
+package navmesh
+
+import (
+	"github.com/kercylan98/minotaur/utils/generic"
+	"github.com/kercylan98/minotaur/utils/geometry"
+)
+
+type funnel[V generic.SignedNumber] struct {
+	path    []geometry.Point[V]
+	portals [][2]geometry.Point[V]
+}
+
+func (slf *funnel[V]) push(point1, point2 geometry.Point[V]) {
+	slf.portals = append(slf.portals, [2]geometry.Point[V]{point1, point2})
+}
+
+func (slf *funnel[V]) pushSingle(point geometry.Point[V]) {
+	slf.portals = append(slf.portals, [2]geometry.Point[V]{point, point})
+}
+
+func (slf *funnel[V]) stringPull() []geometry.Point[V] {
+	var (
+		portals     = slf.portals
+		points      []geometry.Point[V]
+		apexIndex   = 0
+		leftIndex   = 0
+		rightIndex  = 0
+		portalApex  = portals[0][0]
+		portalLeft  = portals[0][0]
+		portalRight = portals[0][1]
+	)
+
+	points = append(points, portalApex)
+	for i := 1; i < len(portals); i++ {
+		lr := portals[i]
+		left, right := lr[0], lr[1]
+
+		if geometry.CalcTriangleTwiceArea(portalApex, portalRight, right) <= V(0) {
+			if portalApex.Equal(portalRight) || geometry.CalcTriangleTwiceArea(portalApex, portalLeft, right) > V(0) {
+				portalRight = right
+				rightIndex = i
+			} else {
+				points = append(points, portalLeft)
+				portalApex = portalLeft
+				apexIndex = leftIndex
+
+				portalLeft = portalApex
+				portalRight = portalApex
+				leftIndex = apexIndex
+				rightIndex = apexIndex
+
+				i = apexIndex
+				continue
+			}
+		}
+
+		if geometry.CalcTriangleTwiceArea(portalApex, portalLeft, left) >= V(0) {
+			if portalApex.Equal(portalLeft) || geometry.CalcTriangleTwiceArea(portalApex, portalRight, left) < V(0) {
+				portalLeft = left
+				leftIndex = i
+			} else {
+				points = append(points, portalRight)
+				portalApex = portalRight
+				apexIndex = rightIndex
+
+				portalLeft = portalApex
+				portalRight = portalApex
+				leftIndex = apexIndex
+				rightIndex = apexIndex
+
+				i = apexIndex
+				continue
+			}
+		}
+	}
+
+	if len(points) == 0 || !points[len(points)-1].Equal(portals[len(portals)-1][0]) {
+		points = append(points, portals[len(portals)-1][0])
+	}
+
+	slf.path = points
+	return slf.path
+}
diff --git a/utils/geometry/navmesh/navmesh.go b/utils/geometry/navmesh/navmesh.go
index 53ea989..4c7b9f7 100644
--- a/utils/geometry/navmesh/navmesh.go
+++ b/utils/geometry/navmesh/navmesh.go
@@ -13,7 +13,7 @@ func NewNavMesh[V generic.SignedNumber](shapes []geometry.Shape[V], meshShrinkAm
 		meshShrinkAmount: meshShrinkAmount,
 	}
 	for i, shape := range shapes {
-		nm.meshShapes[i] = newShape(shape)
+		nm.meshShapes[i] = newShape(i, shape)
 	}
 	nm.generateLink()
 	return nm
@@ -129,6 +129,39 @@ func (slf *NavMesh[V]) FindPath(start, end geometry.Point[V]) (result []geometry
 	}
 
 	path = append([]*shape[V]{startShape}, path...)
+
+	funnel := new(funnel[V])
+	funnel.pushSingle(start)
+	for i := 0; i < len(path)-1; i++ {
+		current := path[i]
+		next := path[i+1]
+
+		var portal geometry.Line[V]
+		var find bool
+		for i := 0; i < len(current.links); i++ {
+			if current.links[i].id == next.id {
+				portal = current.portals[i]
+				find = true
+			}
+		}
+		if !find {
+			panic("not found portal")
+		}
+
+		funnel.push(portal.GetStart(), portal.GetEnd())
+	}
+	funnel.pushSingle(end)
+	funnel.stringPull()
+
+	var lastPoint geometry.Point[V]
+	for i, point := range funnel.path {
+		var np = point.Copy()
+		if i == 0 || !np.Equal(lastPoint) {
+			result = append(result, np)
+		}
+		lastPoint = np
+	}
+	return result
 }
 
 func (slf *NavMesh[V]) generateLink() {
diff --git a/utils/geometry/navmesh/shape.go b/utils/geometry/navmesh/shape.go
index da5d966..9f0ee72 100644
--- a/utils/geometry/navmesh/shape.go
+++ b/utils/geometry/navmesh/shape.go
@@ -5,8 +5,9 @@ import (
 	"github.com/kercylan98/minotaur/utils/geometry"
 )
 
-func newShape[V generic.SignedNumber](s geometry.Shape[V]) *shape[V] {
+func newShape[V generic.SignedNumber](id int, s geometry.Shape[V]) *shape[V] {
 	return &shape[V]{
+		id:             id,
 		Shape:          s,
 		centroid:       geometry.CalcRectangleCentroid(s),
 		boundingRadius: geometry.CalcBoundingRadius(s),
@@ -15,6 +16,7 @@ func newShape[V generic.SignedNumber](s geometry.Shape[V]) *shape[V] {
 }
 
 type shape[V generic.SignedNumber] struct {
+	id int
 	geometry.Shape[V]
 	links          []*shape[V]
 	portals        []geometry.Line[V]