✨ astar实现

utils/astar/astar.go

@@ -1 +1,37 @@
 package astar
+
+import (
+	"container/heap"
+	"github.com/kercylan98/minotaur/utils/generic"
+)
+
+// Find 使用成本函数和成本启发式函数在图中找到起点和终点之间的最低成本路径。
+func Find[Node comparable, V generic.SignedNumber](graph Graph[Node], start, end Node, cost, heuristic func(a, b Node) V) []Node {
+	closed := make(map[Node]bool)
+
+	h := &h[path[Node, V], V]{}
+	heap.Init(h)
+	heap.Push(h, &hm[path[Node, V], V]{v: path[Node, V]{start}})
+
+	for h.Len() > 0 {
+		p := heap.Pop(h).(*hm[path[Node, V], V]).v
+		n := p.last()
+		if closed[n] {
+			continue
+		}
+		if n == end {
+			return p
+		}
+		closed[n] = true
+
+		for _, nb := range graph.Neighbours(n) {
+			cp := p.cont(nb)
+			heap.Push(h, &hm[path[Node, V], V]{
+				v: cp,
+				p: -(cp.cost(cost) + heuristic(nb, end)),
+			})
+		}
+	}
+
+	return nil
+}

utils/astar/graph.go

@@ -0,0 +1,7 @@
+package astar
+
+// Graph 适用于 A* 算法的图数据结构接口定义
+type Graph[Node comparable] interface {
+	// Neighbours 返回特定节点的邻居节点
+	Neighbours(node Node) []Node
+}

utils/astar/heap.go

@@ -0,0 +1,36 @@
+package astar
+
+import "github.com/kercylan98/minotaur/utils/generic"
+
+type hm[T any, V generic.SignedNumber] struct {
+	v T
+	p V
+}
+
+type h[T any, V generic.SignedNumber] []*hm[T, V]
+
+func (slf *h[T, V]) Len() int {
+	return len(*slf)
+}
+
+func (slf *h[T, V]) Less(i, j int) bool {
+	h := *slf
+	return h[i].p > h[j].p
+}
+
+func (slf *h[T, V]) Swap(i, j int) {
+	h := *slf
+	h[i], h[j] = h[j], h[i]
+}
+
+func (slf *h[T, V]) Push(x any) {
+	*slf = append(*slf, x.(*hm[T, V]))
+}
+
+func (slf *h[T, V]) Pop() any {
+	h := *slf
+	size := len(h)
+	t := h[size-1]
+	*slf = h[0 : size-1]
+	return t
+}

utils/astar/node.go

@@ -1,4 +0,0 @@
-package astar
-
-type Node interface {
-}

utils/astar/path.go

@@ -0,0 +1,26 @@
+package astar
+
+import "github.com/kercylan98/minotaur/utils/generic"
+
+type path[Node comparable, V generic.SignedNumber] []Node
+
+// 获取末位元素
+func (p path[Node, V]) last() Node {
+	return p[len(p)-1]
+}
+
+// 将 n 追加到末位，返回新的路径，不影响原始路径
+func (p path[Node, V]) cont(n Node) path[Node, V] {
+	cp := make([]Node, len(p), len(p)+1)
+	copy(cp, p)
+	cp = append(cp, n)
+	return cp
+}
+
+// cost 计算路径总消耗
+func (p path[Node, V]) cost(d func(a, b Node) V) (c V) {
+	for i := 1; i < len(p); i++ {
+		c += d(p[i-1], p[i])
+	}
+	return c
+}