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

package astar0

import (
	"fmt"
	"sort"

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

var (
	straightCost int32 = 10 // 直线的移动成本
	diagonalCost int32 = 14 // 对角线的移动成本
)

type (
	Config struct {
		Width          int32                        // 地图宽(x)
		Height         int32                        // 地图高(y)
		MapData        [][]int32                    // 地图数据
		DirectionFunc  func(node *Node) [][]int32   // 获取各方向节点函数
		IsWalkableFunc func(x, y, value int32) bool // 节点是否可走计算函数
		HeuristicFunc  func(node, end *Node) int32  // 启发算法函数
		IsDebug        bool                         // 是否为调试模式
	}

	AStar struct {
		config    Config    // 配置
		nodes     [][]*Node // 预初始化的节点对象 x,y
		start     *Node     // 开始节点
		end       *Node     // 结束节点
		openList  []*Node   // 开放列表
		closeList []*Node   // 关闭列表
	}
)

func New(config Config) *AStar {
	instance := &AStar{
		config: config,
	}
	instance.init()

	return instance
}

func (p *AStar) init() {
	p.nodes = make([][]*Node, p.config.Width)

	for x := int32(0); x < p.config.Width; x++ {
		p.nodes[x] = make([]*Node, p.config.Height)

		for y := int32(0); y < p.config.Height; y++ {
			p.nodes[x][y] = newNode(x, y, 0)
		}
	}

	for y := int32(0); y < int32(len(p.config.MapData)); y++ {
		for x := int32(0); x < int32(len(p.config.MapData)); x++ {
			if x > p.config.Width || y > p.config.Height {
				continue
			}

			p.nodes[x][y].value = p.config.MapData[y][x]
		}
	}

	p.config.MapData = nil

	if p.config.DirectionFunc == nil {
		p.config.DirectionFunc = Direction8Func
	}

	if p.config.IsWalkableFunc == nil {
		p.config.IsWalkableFunc = IsWalkableFunc()
	}

	if p.config.HeuristicFunc == nil {
		p.config.HeuristicFunc = ManhattanFunc
	}
}

func (p *AStar) SetNodeValue(x, y, value int32) {
	if x < 0 || y < 0 {
		return
	}

	if x > p.config.Width-1 || y > p.config.Height-1 {
		return
	}

	p.nodes[x][y].value = value
}

func (p *AStar) SetStraightCost(value int32) {
	if value > 0 {
		straightCost = value
	}
}

func (p *AStar) SetDiagonalCost(value int32) {
	if value > 0 {
		diagonalCost = value
	}
}

func (p *AStar) calcG(start, end *Node) int32 {
	// 判断移动方式是水平（或垂直）、对角，计算成本
	if start.x == end.x || start.y == end.y {
		return straightCost
	}

	return diagonalCost
}

func (p *AStar) calcH(start, end *Node) int32 {
	return p.config.HeuristicFunc(start, end)
}

func (p *AStar) FindPath(sx, sy, ex, ey int32) ([]*Node, error) {
	end, err := p.findPath(sx, sy, ex, ey)
	if err != nil {
		return nil, err
	}

	pathList := BuildPathing(end)
	p.Reset()

	return pathList, nil
}

func (p *AStar) CheckNodes() {
	for x, n1 := range p.nodes {
		for y := range n1 {
			node := p.nodes[x][y]

			if node.status > 0 {
				fmt.Println("status:", node)
			}

			if node.parent != nil {
				fmt.Println("parent:", node, node.parent)
			}
		}
	}
}

func (p *AStar) findPath(sx, sy, ex, ey int32) (*Node, error) {
	if !p.isWalkable(sx, sy) {
		return nil, ErrStartNotWalkable
	}

	if !p.isWalkable(ex, ey) {
		return nil, ErrEndNotWalkable
	}

	p.start = p.nodes[sx][sy]
	p.end = p.nodes[ex][ey]

	p.addOpenNode(p.start)

	for len(p.openList) > 0 {
		openNode := p.openList[0]
		p.openList = p.openList[1:]

		if p.isEnd(openNode) {
			return openNode, nil
		}

		nodes := p.getNodes(openNode)
		if p.config.IsDebug {
			clog.Infof("%v -> %v", openNode, nodes)
		}

		for _, node := range nodes {
			g := openNode.g + p.calcG(openNode, node)

			if !node.IsOpen() || g < node.g {
				node.g = g
				node.h = p.calcH(node, p.end)
				node.f = node.g + node.h
				node.parent = openNode

				if !node.IsOpen() {
					p.addOpenNode(node)
				}
			}
		}

		p.addCloseNode(openNode)

		sort.Slice(p.openList, func(i, j int) bool {
			return p.openList[i].f < p.openList[j].f
		})
	}

	return nil, ErrFindPathIsNil
}

func (p *AStar) Reset() {
	for _, node := range p.openList {
		node.Clean()
	}

	for _, node := range p.closeList {
		node.Clean()
	}

	p.start.Clean()
	p.end.Clean()

	p.start = nil
	p.end = nil
	p.openList = nil
	p.closeList = nil
}

func (p *AStar) addOpenNode(node *Node) {
	node.status = statusOpen
	p.openList = append(p.openList, node)
}

func (p *AStar) addCloseNode(node *Node) {
	node.status = statusClose
	p.closeList = append(p.closeList, node)
}

func (p *AStar) getNodes(node *Node) []*Node {
	var (
		list          []*Node
		directionList = p.config.DirectionFunc(node)
	)

	for _, v := range directionList {
		x, y := node.x+v[0], node.y+v[1]
		if !p.isWalkable(x, y) {
			continue
		}

		tmpNode := p.nodes[x][y]
		if tmpNode.IsClose() {
			continue
		}

		list = append(list, tmpNode)
	}

	return list
}

func (p *AStar) CheckPoint(x, y int32) bool {
	if x < 0 || y < 0 {
		return false
	}

	if x > p.config.Width-1 || y > p.config.Height-1 {
		return false
	}

	return true
}

func (p *AStar) isWalkable(x, y int32) bool {
	if !p.CheckPoint(x, y) {
		return false
	}

	return p.config.IsWalkableFunc(x, y, p.nodes[x][y].value)
}

func (p *AStar) isEnd(node *Node) bool {
	return node.x == p.end.x && node.y == p.end.y
}

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

	var (
		fromNode               = list[0]
		toNode                 = list[len(list)-1]
		minX, minY, maxX, maxY = GetRectanglePoint(list)
	)

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

	for y := minY; y <= maxY; y++ {
		for x := minX; x <= maxX; x++ {
			if InList(list, x, 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.nodes[x][y].value)
			}
		}
		mapText += "\n"
	}

	clog.Debug(mapText)
}