✨ 网格寻路寻找最近的点

utils/geometry/navmesh/navmesh.go

@@ -6,8 +6,50 @@ import (
 	"github.com/kercylan98/minotaur/utils/maths"
 )
 
+func NewNavMesh[V generic.SignedNumber](shapes []geometry.Shape[V], meshShrinkAmount V) *NavMesh[V] {
+	nm := &NavMesh[V]{
+		meshShapes:       make([]*shape[V], len(shapes)),
+		meshShrinkAmount: meshShrinkAmount,
+	}
+	for i, shape := range shapes {
+		nm.meshShapes[i] = newShape(shape)
+	}
+	nm.generateLink()
+	return nm
+}
+
 type NavMesh[V generic.SignedNumber] struct {
-	meshShapes []*shape[V]
+	meshShapes       []*shape[V]
+	meshShrinkAmount V
+}
+
+// Find 在网格中找到与给定点最近的点。如果该点已经在网格中，这将为您提供该点。如果点在网格之外，这将尝试将此点投影到网格中（直到给定的 maxDistance）
+func (slf *NavMesh[V]) Find(point geometry.Point[V], maxDistance V) (distance V, findPoint geometry.Point[V], findShape geometry.Shape[V]) {
+	var minDistance = maxDistance
+	var closest *shape[V]
+	var pointOnClosest geometry.Point[V]
+	for _, meshShape := range slf.meshShapes {
+		if meshShape.Contains(point) || geometry.IsPointOnEdge(meshShape.Edges(), point) {
+			minDistance = 0
+			closest = meshShape
+			pointOnClosest = point
+			break
+		}
+		br := geometry.CalcBoundingRadius(meshShape.Shape)
+		distance := geometry.CalcDistance(geometry.DoublePointToCoordinate(
+			geometry.CalcRectangleCentroid(meshShape.Shape),
+			point,
+		))
+		if distance-br < minDistance {
+			point, distance := geometry.ProjectionPointToShape(point, meshShape.Shape)
+			if distance < minDistance {
+				minDistance = distance
+				closest = meshShape
+				pointOnClosest = point
+			}
+		}
+	}
+	return minDistance, pointOnClosest, closest.Shape
 }
 
 func (slf *NavMesh[V]) generateLink() {

utils/geometry/position.go

@@ -166,8 +166,8 @@ func DoublePointToCoordinate[V generic.SignedNumber](point1, point2 Point[V]) (x
 
 // CalcProjectionPoint 计算一个点到一条线段的最近点（即投影点）的。这个函数接收一个点和一条线段作为输入，线段由两个端点组成。
 //   - 该函数的主要用于需要计算一个点到一条线段的最近点的情况下
-func CalcProjectionPoint[V generic.SignedNumber](linePointA, linePointB, point Point[V]) Point[V] {
-	ax, ay, bx, by := DoublePointToCoordinate(linePointA, linePointB)
+func CalcProjectionPoint[V generic.SignedNumber](line Line[V], point Point[V]) Point[V] {
+	ax, ay, bx, by := DoublePointToCoordinate(line.GetStart(), line.GetEnd())
 	ds := CalcDistanceSquared(ax, ay, bx, by)
 	px, py := point.GetXY()
 	clamp := maths.Clamp((px-ax)*(bx-ax)+(py-ay)*(by-ay)/ds, V(0), V(1))

utils/geometry/shape.go

@@ -67,6 +67,23 @@ func (slf Shape[V]) PointCount() int {
 	return len(slf)
 }
 
+// Contains 返回该形状中是否包含点
+func (slf Shape[V]) Contains(point Point[V]) bool {
+	x, y := point.GetXY()
+	inside := false
+	for i, j := -1, len(slf)-1; i < len(slf); j, i = i, i+1 {
+		ix := slf[i].GetX()
+		iy := slf[i].GetY()
+		jx := slf[j].GetX()
+		jy := slf[j].GetY()
+
+		if ((iy <= y && y < jy) || (jy <= y && y < iy)) && x < ((jx-ix)*(y-iy))/(jy-iy)+ix {
+			inside = !inside
+		}
+	}
+	return inside
+}
+
 // String 将该形状转换为可视化的字符串进行返回
 func (slf Shape[V]) String() string {
 	var result string
@@ -358,6 +375,16 @@ func (slf Shape[V]) Edges() (edges []Line[V]) {
 	return edges
 }
 
+// IsPointOnEdge 检查点是否在该形状的一条边上
+func (slf Shape[V]) IsPointOnEdge(point Point[V]) bool {
+	for _, edge := range slf.Edges() {
+		if PointOnSegmentWithCoordinateArray(edge.GetStart(), edge.GetEnd(), point) {
+			return true
+		}
+	}
+	return false
+}
+
 // getAllGraphicCompositionWithAsc 通过升序的方式获取该形状中包含的所有图形组合及其位置
 //   - 升序指标为图形包含的点数量
 //   - 其余内容可参考 getAllGraphicComposition
@@ -413,3 +440,32 @@ func CalcTriangleTwiceArea[V generic.SignedNumber](a, b, c Point[V]) V {
 	by := c.GetY() - a.GetY()
 	return bx*ay - ax*by
 }
+
+// IsPointOnEdge 检查点是否在 edges 的任意一条边上
+func IsPointOnEdge[V generic.SignedNumber](edges []Line[V], point Point[V]) bool {
+	for _, edge := range edges {
+		if PointOnSegmentWithCoordinateArray(edge.GetStart(), edge.GetEnd(), point) {
+			return true
+		}
+	}
+	return false
+}
+
+// ProjectionPointToShape 将一个点投影到一个多边形上，找到离该点最近的投影点，并返回投影点和距离
+func ProjectionPointToShape[V generic.SignedNumber](point Point[V], shape Shape[V]) (Point[V], V) {
+	var closestProjection Point[V]
+	var hasClosestProjection bool
+	var closestDistance V
+
+	for _, edge := range shape.Edges() {
+		projectedPoint := CalcProjectionPoint(edge, point)
+		distance := CalcDistance(DoublePointToCoordinate(point, projectedPoint))
+		if !hasClosestProjection || distance < closestDistance {
+			closestDistance = distance
+			closestProjection = projectedPoint
+			hasClosestProjection = true
+		}
+	}
+
+	return closestProjection, closestDistance
+}