s_cherry_e/sr/internal/algorithm/astar/astar.go

package astar

import (
	"f1-game/internal/constant"
	"f1-game/internal/types"
	"fmt"
	"math"
	"slices"
	"sort"

	clog "github.com/cherry-game/cherry/logger"
)

// AStar A*寻路算法结构
type AStar struct {
	config    Config
	cache     map[int32]*Node
	openList  []*Node
	closeList types.Set[*Node]
	parentMap map[*Node]*Node // 当前寻路的父节点
	gMap      map[*Node]int32 // 当前寻路的g值
	hMap      map[*Node]int32 // 当前寻路的h值
	// mapData   [][]int32
}

func New(c Config) *AStar {
	// if c.Row <= 0 || c.Column <= 0 {
	// 	clog.Fatal("map width or height must be greater than 0")
	// }

	// if len(c.MapData) == 0 {
	// 	clog.Fatal("map data is empty")
	// }

	// if c.Column > int32(len(c.MapData)) || c.Row > int32(len(c.MapData[0])) {
	// 	clog.Fatal("map width or height must be less than map data size")
	// }

	instance := &AStar{config: c}
	instance.init()

	return instance
}

func (p *AStar) init() {
	p.closeList = types.NewSet[*Node]()
	p.parentMap = make(map[*Node]*Node)
	p.gMap = make(map[*Node]int32)
	p.hMap = make(map[*Node]int32)

	p.cache = make(map[int32]*Node)
	for x := range p.config.Width {
		for y := range p.config.Height {
			value := p.config.MapData[x][y]
			if constant.IsWalkableTile(value) {
				// 加上偏移量
				p.addNode(NewNode(x, y, true))
			}
		}
	}

	p.initNodeNeighbors()
}

func (p *AStar) initNodeNeighbors() {
	for x := range p.config.Width {
		for y := range p.config.Height {
			node := p.getNode(x, y)
			if node == nil || !node.isWalkable {
				continue
			}

			if node.IsEven() { // 偶数列
				// x + 1
				node.AddNeighbor(p.getNode(x+1, y), 14)
				node.AddNeighbor(p.getNode(x+1, y+1), 14)
				// x - 1
				node.AddNeighbor(p.getNode(x-1, y), 14)
				node.AddNeighbor(p.getNode(x-1, y+1), 14)
				// x
				node.AddNeighbor(p.getNode(x, y-1), 10)
				node.AddNeighbor(p.getNode(x, y+1), 10)
			} else { // 奇数列
				// x + 1
				node.AddNeighbor(p.getNode(x+1, y), 11)
				node.AddNeighbor(p.getNode(x+1, y-1), 11)
				// x - 1
				node.AddNeighbor(p.getNode(x-1, y), 11)
				node.AddNeighbor(p.getNode(x-1, y-1), 11)
				// x
				node.AddNeighbor(p.getNode(x, y-1), 10)
				node.AddNeighbor(p.getNode(x, y+1), 10)
			}
		}
	}
}

func (p *AStar) GetDistance(sx int32, sy int32, ex int32, ey int32) int32 {
	var (
		start = p.getNode(sx, sy)
		end   = p.getNode(ex, ey)
	)

	if start == nil || end == nil {
		return math.MaxInt32
	}

	return p.calDistance(start, end)
}

func (p *AStar) ResetCache() {
	for _, node := range p.cache {
		node.isWalkable = true
	}
}

func (p *AStar) SetObstacles(x, y int32, isWalkable bool) {
	id := p.nodeID(x, y)
	node := p.cache[id]
	if node != nil {
		node.isWalkable = isWalkable
	}
}

// FindPath 寻路入口函数
func (p *AStar) FindPath(x1, y1, x2, y2 int32) ([]*Node, bool) {
	var (
		start = p.getNode(x1, y1)
		end   = p.getNode(x2, y2)
	)

	if !p.isWalkable(start) || !p.isWalkable(end) {
		clog.Debugf("path find failed: start or end is not walkable: (%v -> %v)", start, end)
		return nil, false
	}

	// 清理状态
	p.clear()

	// 将起点加入开启列表
	p.openList = append(p.openList, start)

	result := p.search(end)
	if len(result) == 0 {
		clog.Debugf("path find failed: find empty path: (%v -> %v)", start, end)
		return nil, false
	}

	return result, true
}

// search 寻路核心算法
func (p *AStar) search(end *Node) []*Node {
	var (
		found   bool
		current *Node
	)

	for len(p.openList) > 0 {
		current = p.currNode()

		// 找到终点
		if current.x == end.x && current.y == end.y {
			found = true
			break
		}

		for neighbor, cost := range current.neighbors {
			if p.closeList.Contains(neighbor) {
				continue
			}

			// 跳过不可行走的节点（可能在寻路过程中被动态设置为不可行走）
			if !p.isWalkable(neighbor) {
				continue
			}

			p.checkPath(neighbor, current, end, cost)
		}

		// 按F值排序开启列表
		p.sortOpenList()
	}

	if found {
		return p.buildPath(current)
	}

	return nil
}

func (p *AStar) currNode() *Node {
	node := p.openList[0]

	p.openList = p.openList[1:]
	p.closeList.Add(node)

	return node
}

func (p *AStar) clear() {
	p.openList = p.openList[:0]
	p.closeList = types.NewSet[*Node]()
	p.parentMap = make(map[*Node]*Node)
	p.gMap = make(map[*Node]int32)
	p.hMap = make(map[*Node]int32)
}

// isWalkable 检查节点是否可通行
func (p *AStar) isWalkable(node *Node) bool {
	// // 出界
	// if p.outOfBounds(x, y) {
	// 	return false
	// }

	if node == nil || !node.isWalkable {
		return false
	}

	// 否则使用自定义函数
	if p.config.IsWalkableFunc != nil {
		return p.config.IsWalkableFunc(node.x, node.y)
	}

	return true
}

// func (p *AStar) outOfBounds(x, y int32) bool {
// 	return x < p.config.Min.X ||
// 		x >= p.config.Max.X ||
// 		y < p.config.Min.Y ||
// 		y >= p.config.Max.Y
// }

func (p *AStar) sortOpenList() {
	sort.Slice(p.openList, func(i, j int) bool {
		return p.getF(p.openList[i]) < p.getF(p.openList[j])
	})
}

func (p *AStar) getF(node *Node) int32 {
	return p.gMap[node] + p.hMap[node]
}

// checkPath 检查路径是否可行
func (p *AStar) checkPath(node, parent, end *Node, cost int32) bool {
	// 检查是否在开启列表中
	if p.inOpenList(node) {
		if p.gMap[node]+cost < p.gMap[node] {
			p.parentMap[node] = parent
			{
				p.gMap[node] = p.calG(node, parent, cost)
				p.hMap[node] = p.calDistance(node, end)
			}
		}
	} else {
		p.parentMap[node] = parent
		{
			p.gMap[node] = p.calG(node, parent, cost)
			p.hMap[node] = p.calDistance(node, end)
		}
		p.openList = append(p.openList, node)
	}

	return true
}

// buildPath 构建路径
func (p *AStar) buildPath(node *Node) []*Node {
	var result []*Node

	// 从终点回溯到起点
	for node != nil {
		result = append(result, node)
		node = p.parentMap[node]
	}

	// 翻转路径
	slices.Reverse(result)

	return result
}

// 辅助函数：检查节点是否在开启/关闭列表中
func (p *AStar) inOpenList(node *Node) bool {
	return slices.Contains(p.openList, node)
}

func (p *AStar) ContainsNode(x, y int32) bool {
	return p.getNode(x, y) != nil
}

func (p *AStar) getNode(x, y int32) *Node {
	nodeID := p.nodeID(x, y)
	return p.cache[nodeID]
}

func (p *AStar) addNode(node *Node) {
	id := p.nodeID(node.x, node.y)
	p.cache[id] = node
}

func (p *AStar) nodeID(x, y int32) int32 {
	return y*p.config.Width + x
}

func (p *AStar) calDistance(node1, node2 *Node) int32 {
	dx := int32(math.Abs(float64(node1.x - node2.x)))
	dy := int32(math.Abs(float64(node1.y - node2.y)))
	return dx + dy
}

func (p *AStar) calG(node, parent *Node, cost int32) int32 {
	if parent == nil {
		return cost
	}

	var (
		grandpa = p.parentMap[parent]
		px      = parent.x
		py      = parent.y
		gx      = parent.x
		gy      = parent.y
	)

	if grandpa != nil {
		gx = grandpa.x
		gy = grandpa.y
	}

	var (
		pdx        = px - gx
		pdy        = py - gy
		dx         = node.x - px
		dy         = node.y - py
		turnPenlty = p.calTurnPenlty(dx, dy, pdx, pdy, node.IsOdd())
	)

	return p.gMap[parent] + cost + turnPenlty
}

func (p *AStar) calTurnPenlty(dx, dy, pdx, pdy int32, isOdd bool) int32 {
	if p.isTurn(dx, dy, pdx, pdy, isOdd) {
		return Cost_Turn
	}

	return 0
}

func (p *AStar) isTurn(dx, dy, pdx, pdy int32, isOdd bool) bool {
	value := dx * pdx
	if value == 0 {
		return false
	}

	if value > 0 {
		if isOdd {
			return pdy != dy+1
		}
		return pdy != dy-1
	}

	return true
}

func (p *AStar) Print(list []*Node) {
	if len(list) < 1 {
		return
	}

	var (
		fromNode = list[0]
		toNode   = list[len(list)-1]
	)

	mapText := "\n"
	mapText += fmt.Sprintf(" Rectangle: [(%d,%d) -> (%d,%d)] \n", fromNode.x, fromNode.y, toNode.x, toNode.y)
	mapText += fmt.Sprintf("   Pathing: %v\n", list)
	mapText += "   Pattern: start = ◉, point = ◦, end = ●\n"
	mapText += "Coordinate:   row = x,   col = y\n\n"

	for x := range p.config.Width {
		if x%2 == 0 {
			mapText += " "
		}
		for y := range p.config.Height {
			if slices.ContainsFunc(list, func(node *Node) bool { return node.x == x && node.y == y }) {
				if x == fromNode.x && y == fromNode.y {
					mapText += "◉ "
				} else if x == toNode.x && y == toNode.y {
					mapText += "● "
				} else {
					mapText += "◦ "
				}
			} else {
				mapText += fmt.Sprintf("%d ", p.config.MapData[x][y])
			}
		}
		mapText += "\n"
	}

	clog.Debug(mapText)
}