《游戏服务端编程实践》7.2.1 A*、Dijkstra 算法原理

// file: ch07/path/astar.go
// tier: L1
// verify: go test ./ch07/path/

package path

import (
	"container/heap"
	"errors"
	"math"
)

var (
	ErrNoPath      = errors.New("path: no route")
	ErrOutOfBounds = errors.New("path: out of bounds")
	ErrBlocked     = errors.New("path: endpoint blocked")
)

// Point 是整数格子坐标，X 向右、Y 向下。
type Point struct{ X, Y int }

// Grid 提供地形成本；Cost 的第二个返回值表示格子能否进入，
// MinCost 是任意可进入格子的最低成本，必须大于 0。
type Grid interface {
	Width() int
	Height() int
	Cost(p Point) (float64, bool)
	MinCost() float64
}

type Result struct {
	Path     []Point
	Cost     float64
	Expanded int
}

// Searcher 预分配与地图等大的数组，同一张地图上的多次寻路复用它们。
type Searcher struct {
	grid      Grid
	heuristic func(Point, Point) float64
	w, h      int
	gScore    []float64
	cameFrom  []int
	seen      []uint32
	epoch     uint32
	open      nodeHeap
	buf       []Point
}

// NewSearcher 的默认启发函数是欧氏距离乘以最低地形成本，保证不高估。
func NewSearcher(g Grid) *Searcher {
	minCost := g.MinCost()
	if minCost <= 0 {
		minCost = 1
	}
	s := &Searcher{grid: g, w: g.Width(), h: g.Height()}
	s.heuristic = func(a, b Point) float64 {
		return math.Hypot(float64(a.X-b.X), float64(a.Y-b.Y)) * minCost
	}
	n := s.w * s.h
	s.gScore = make([]float64, n)
	s.cameFrom = make([]int, n)
	s.seen = make([]uint32, n)
	return s
}

// Dijkstra 等价于零启发函数的 A*，用于对照最短路。
func Dijkstra(g Grid, start, goal Point) (Result, error) {
	s := NewSearcher(g)
	s.heuristic = func(Point, Point) float64 { return 0 }
	return s.Find(start, goal)
}

func (s *Searcher) Find(start, goal Point) (Result, error) {
	if !s.inBounds(start) || !s.inBounds(goal) {
		return Result{}, ErrOutOfBounds
	}
	if _, ok := s.grid.Cost(start); !ok {
		return Result{}, ErrBlocked
	}
	if _, ok := s.grid.Cost(goal); !ok {
		return Result{}, ErrBlocked
	}
	s.epoch++
	if s.epoch == 0 {
		clear(s.seen)
		s.epoch = 1
	}
	startIdx, goalIdx := s.index(start), s.index(goal)
	s.seen[startIdx] = s.epoch
	s.gScore[startIdx] = 0
	s.cameFrom[startIdx] = -1
	s.open = s.open[:0]
	heap.Push(&s.open, node{idx: startIdx, f: s.heuristic(start, goal)})
	expanded := 0
	for s.open.Len() > 0 {
		cur := heap.Pop(&s.open).(node)
		if s.seen[cur.idx] != s.epoch || cur.g > s.gScore[cur.idx] {
			continue
		}
		expanded++
		if cur.idx == goalIdx {
			return s.build(startIdx, goalIdx, cur.g, expanded), nil
		}
		cp := s.point(cur.idx)
		for _, nb := range s.neighbors(cp) {
			ng := cur.g + s.stepCost(cp, nb)
			ni := s.index(nb)
			if s.seen[ni] == s.epoch && ng >= s.gScore[ni] {
				continue
			}
			s.seen[ni] = s.epoch
			s.gScore[ni] = ng
			s.cameFrom[ni] = cur.idx
			heap.Push(&s.open, node{idx: ni, g: ng, f: ng + s.heuristic(nb, goal)})
		}
	}
	return Result{}, ErrNoPath
}

var directions = [8][2]int{
	{1, 0}, {-1, 0}, {0, 1}, {0, -1},
	{1, 1}, {1, -1}, {-1, 1}, {-1, -1},
}

func (s *Searcher) neighbors(p Point) []Point {
	out := s.buf[:0]
	for i, d := range directions {
		q := Point{p.X + d[0], p.Y + d[1]}
		if !s.inBounds(q) {
			continue
		}
		if _, ok := s.grid.Cost(q); !ok {
			continue
		}
		// 斜向要求两侧正交格都可通行，禁止从墙角挤过去。
		if i >= 4 {
			if _, ok := s.grid.Cost(Point{p.X + d[0], p.Y}); !ok {
				continue
			}
			if _, ok := s.grid.Cost(Point{p.X, p.Y + d[1]}); !ok {
				continue
			}
		}
		out = append(out, q)
	}
	s.buf = out
	return out
}

func (s *Searcher) stepCost(a, b Point) float64 {
	ca, _ := s.grid.Cost(a)
	cb, _ := s.grid.Cost(b)
	if a.X != b.X && a.Y != b.Y {
		return math.Sqrt2 * math.Max(ca, cb)
	}
	return cb
}

func (s *Searcher) build(start, goal int, cost float64, expanded int) Result {
	var reversed []Point
	for at := goal; at != -1; at = s.cameFrom[at] {
		reversed = append(reversed, s.point(at))
		if at == start {
			break
		}
	}
	path := make([]Point, len(reversed))
	for i := range reversed {
		path[len(reversed)-1-i] = reversed[i]
	}
	return Result{Path: path, Cost: cost, Expanded: expanded}
}

func (s *Searcher) index(p Point) int { return p.Y*s.w + p.X }
func (s *Searcher) point(i int) Point { return Point{X: i % s.w, Y: i / s.w} }
func (s *Searcher) inBounds(p Point) bool {
	return p.X >= 0 && p.Y >= 0 && p.X < s.w && p.Y < s.h
}

type node struct {
	idx int
	g   float64
	f   float64
}

type nodeHeap []node

func (h nodeHeap) Len() int { return len(h) }
func (h nodeHeap) Less(i, j int) bool {
	if h[i].f == h[j].f {
		return h[i].g > h[j].g
	}
	return h[i].f < h[j].f
}
func (h nodeHeap) Swap(i, j int) { h[i], h[j] = h[j], h[i] }
func (h *nodeHeap) Push(x any)   { *h = append(*h, x.(node)) }
func (h *nodeHeap) Pop() any {
	old := *h
	n := len(old)
	it := old[n-1]
	*h = old[:n-1]
	return it
}

cd .local/checks/gs-practice-lab && gofmt -l ch07/path && go vet ./ch07/path/ && go test ./ch07/path/ -count=1

ok  	gspractice/ch07/path	0.262s