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

// Package astar 实现了六边形网格的A*寻路算法
// 采用odd-q offset坐标系（奇数列向上偏移），支持并发安全调用
// 核心优化：预计算坐标、heap优先队列、对象池复用、视觉直线性惩罚
package astar6

import (
	"container/heap"
	"f1-game/internal/constant"
	"fmt"
	"math"
	"slices"

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

// AStar A*寻路算法结构（无状态，可安全被多协程共享）
type AStar struct {
	config Config
	cache  [][]*Node // 节点缓存，二维数组直接索引（只读，初始化后不变）
}

// New 创建A*寻路实例
// 初始化时会构建所有可行走节点及其邻居关系
// 返回的实例是并发安全的，可被多个goroutine同时调用FindPath
func New(c Config) *AStar {
	instance := &AStar{config: c}
	instance.init()
	return instance
}

// init 初始化寻路器
// 1. 遍历地图数据创建所有可行走节点
// 2. 建立节点间的邻居关系
// 3. 设置默认的关键点提取函数
func (p *AStar) init() {
	// 初始化二维数组缓存
	rows := p.config.Max.X - p.config.Min.X + 1
	cols := p.config.Max.Y - p.config.Min.Y + 1
	p.cache = make([][]*Node, rows)
	for i := range p.cache {
		p.cache[i] = make([]*Node, cols)
	}
	// 遍历地图范围，创建可行走节点
	for x := p.config.Min.X; x <= p.config.Max.X; x++ {
		for y := p.config.Min.Y; y <= p.config.Max.Y; y++ {
			value := p.config.MapData[x-p.config.Min.X][y-p.config.Min.Y]
			if constant.IsWalkableTile(value) {
				p.addNode(NewNode(x, y, true))
			}
		}
	}
	// 建立邻居关系（六边形拓扑）
	p.initNodeNeighbors()
}

// 测试用，设置所有节点为可行走
func (p *AStar) ResetCache() {
	for _, row := range p.cache {
		for _, node := range row {
			if node != nil {
				node.isWalkable = true
			}
		}
	}
}

func (p *AStar) SetNode(x, y int32, isWalkable bool) {
	node := p.GetNode(x, y)
	if node != nil {
		node.SetWalkable(isWalkable)
	}
}

// initNodeNeighbors 初始化所有节点的邻居关系
// 六边形网格采用odd-q坐标系（奇数列上移）
//
// 偶数列(x=0,2,4...)的6个邻居:
//
//	    (x-1,y) (x+1,y)
//	(x,y-1) [x,y] (x,y+1)
//	  (x-1,y+1) (x+1,y+1)
//
// 奇数列(x=1,3,5...)的6个邻居:
//
//	  (x-1,y-1) (x+1,y-1)
//	(x,y-1) [x,y] (x,y+1)
//	    (x-1,y) (x+1,y)
func (p *AStar) initNodeNeighbors() {
	for x := p.config.Min.X; x <= p.config.Max.X; x++ {
		for y := p.config.Min.Y; y <= p.config.Max.Y; y++ {
			node := p.GetNode(x, y)
			if node == nil || !node.isWalkable {
				continue
			}

			if node.IsEven() { // 偶数列
				node.addNeighbor(p.GetNode(x+1, y), ODD_DIAGONAL_DISTINCT)   // 右上
				node.addNeighbor(p.GetNode(x+1, y+1), ODD_DIAGONAL_DISTINCT) // 右下
				node.addNeighbor(p.GetNode(x-1, y), ODD_DIAGONAL_DISTINCT)   // 左上
				node.addNeighbor(p.GetNode(x-1, y+1), ODD_DIAGONAL_DISTINCT) // 左下
				node.addNeighbor(p.GetNode(x, y-1), ODD_STRAIGHT_DISTINCT)   // 上
				node.addNeighbor(p.GetNode(x, y+1), ODD_STRAIGHT_DISTINCT)   // 下
			} else { // 奇数列
				node.addNeighbor(p.GetNode(x+1, y), EVEN_DIAGONAL_DISTINCT)   // 右下
				node.addNeighbor(p.GetNode(x+1, y-1), EVEN_DIAGONAL_DISTINCT) // 右上
				node.addNeighbor(p.GetNode(x-1, y), EVEN_DIAGONAL_DISTINCT)   // 左下
				node.addNeighbor(p.GetNode(x-1, y-1), EVEN_DIAGONAL_DISTINCT) // 左上
				node.addNeighbor(p.GetNode(x, y-1), EVEN_STRAIGHT_DISTINCT)   // 上
				node.addNeighbor(p.GetNode(x, y+1), EVEN_STRAIGHT_DISTINCT)   // 下
			}
		}
	}
}

// FindPath 寻路入口函数（并发安全）
// 从(x1,y1)到(x2,y2)寻找最优路径
// 返回路径节点列表和是否找到路径
// 使用对象池复用搜索上下文，支持多goroutine并发调用
func (p *AStar) FindPath(ctx *FindContext) ([]*Node, bool) {
	// 回收 Context 对象
	defer releaseContext(ctx)

	start := p.GetNode(ctx.sx, ctx.sy)
	end := p.GetNode(ctx.ex, ctx.ey)

	// 验证起点和终点是否可行走
	if !p.isWalkable(start, ctx) || !p.isWalkable(end, ctx) {
		clog.Debugf("path find failed: start or end is not walkable: (%v -> %v)", start, end)
		return nil, false
	}

	// 初始化搜索状态（用于视觉惩罚计算）
	ctx.start = start
	ctx.end = end

	// 初始化起点：g=0，f=h
	ctx.gMap[start] = 0
	h := p.calH(start, ctx.end)
	item := &heapItem{
		node: start,
		f:    h,
	}
	heap.Push(&ctx.openHeap, item)
	ctx.openSet[start] = item

	// 执行A*搜索
	result := p.search(ctx)
	if len(result) == 0 {
		clog.Debugf("path find failed: find empty path: (%v -> %v)", start, end)
		return nil, false
	}

	return result, true
}

// search 寻路核心算法
// 使用A*算法搜索从起点到终点的最优路径
// 算法流程：
// 1. 从openHeap取出f值最小的节点作为当前节点
// 2. 如果当前节点是终点，构建并返回路径
// 3. 将当前节点加入closeSet
// 4. 遍历所有邻居，更新路径代价
// 5. 重复直到找到终点或openHeap为空
func (p *AStar) search(ctx *FindContext) []*Node {
	for ctx.openHeap.Len() > 0 {
		// 从优先队列取出f值最小的节点
		item := heap.Pop(&ctx.openHeap).(*heapItem)
		current := item.node
		delete(ctx.openSet, current)
		ctx.closeSet[current] = struct{}{}

		// 到达终点，构建路径返回
		if current == ctx.end {
			return p.buildPath(ctx, current)
		}

		// 遍历所有邻居节点（优化：使用静态数组）
		neighborCount := current.neighborCount
		for i := int32(0); i < neighborCount; i++ {
			neighborNode := current.neighborNodes[i]
			cost := current.neighborCosts[i]

			// 跳过已在closeSet中的节点
			if _, closed := ctx.closeSet[neighborNode]; closed {
				continue
			}

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

			// 检查并更新邻居节点的路径
			p.checkPath(ctx, neighborNode, current, cost)
		}
	}

	return nil
}

// checkPath 检查并更新节点路径
// 核心逻辑：
// - 如果节点已在openSet中，检查新路径是否更优
// - 如果节点不在openSet中，添加到openSet
// 参数：
// - node: 待检查的邻居节点
// - parent: 当前节点（作为node的父节点）
// - cost: 从parent到node的移动代价
// 注：终点信息来自 ctx.end
// 优化：使用heapItem对象池减少内存分配
func (p *AStar) checkPath(ctx *FindContext, node, parent *Node, cost int32) {
	// 计算经过parent到达node的g值
	newG := p.calG(ctx, node, parent, cost)

	if item, inOpen := ctx.openSet[node]; inOpen {
		// 节点已在openSet中，检查新路径是否更优
		if newG < ctx.gMap[node] {
			ctx.parentMap[node] = parent
			ctx.gMap[node] = newG
			// 使用索引堆O(1)定位，O(log n)更新
			newF := newG + p.calH(node, ctx.end)
			item.f = newF
			heap.Fix(&ctx.openHeap, item.index) // O(log n)复杂度调整堆
		}
	} else {
		// 新节点，添加到openSet
		// 优化：从对象池获取heapItem
		item := getHeapItem()
		item.node = node
		item.f = newG + p.calH(node, ctx.end)

		ctx.parentMap[node] = parent
		ctx.gMap[node] = newG
		heap.Push(&ctx.openHeap, item)
		ctx.openSet[node] = item
	}
}

// buildPath 从终点回溯构建完整路径
// 通过parentMap逆向遍历，直接构建正向路径，避免使用slices.Reverse
func (p *AStar) buildPath(ctx *FindContext, node *Node) []*Node {
	// 先计算路径长度
	count := 0
	for n := node; n != nil; n = ctx.parentMap[n] {
		count++
	}

	// 预分配容量，从后往前填充
	result := make([]*Node, count)
	idx := count - 1
	for n := node; n != nil; n = ctx.parentMap[n] {
		result[idx] = n
		idx--
	}
	return result
}

// isWalkable 判断节点是否可行走
// 优先使用配置的自定义判断函数，否则使用节点自身的isWalkable属性
// 内联到调用处以提升性能
func (p *AStar) isWalkable(node *Node, ctx *FindContext) bool {
	if node == nil || !node.isWalkable {
		return false
	}
	if p.config.IsWalkableFunc != nil {
		return p.config.IsWalkableFunc(node, ctx)
	}
	return true
}

// ContainsNode 检查坐标是否存在对应的节点
func (p *AStar) ContainsNode(x, y int32) bool {
	return p.GetNode(x, y) != nil
}

// GetNode 根据坐标获取节点
// 优化：使用预计算的常量减少运行时边界检查开销
func (p *AStar) GetNode(x, y int32) *Node {
	// 预计算边界检查
	maxX := int32(len(p.cache))
	if maxX == 0 {
		return nil
	}
	maxY := int32(len(p.cache[0]))

	// 计算索引
	idx := x - p.config.Min.X
	idy := y - p.config.Min.Y

	// 优化：合并边界检查
	if idx < 0 || idx >= maxX || idy < 0 || idy >= maxY {
		return nil
	}
	return p.cache[idx][idy]
}

// addNode 添加节点到缓存
func (p *AStar) addNode(node *Node) {
	idx := node.x - p.config.Min.X
	idy := node.y - p.config.Min.Y
	p.cache[idx][idy] = node
}

// calH 计算启发式距离（曼哈顿距离）
// 使用预计算的cube坐标，避免运行时转换开销
// cube坐标下的距离公式: (|x1-x2| + |y1-y2| + |z1-z2|) / 2
//
// 启发式强化策略：
// - 短距离（0-50步）：使用基础曼哈顿距离
// - 中等距离（50-300步）：乘以1.2加强引导（常见游戏寻路距离）
// - 长距离（>300步）：乘以1.1避免过度估计（保持A*最优性）
// 优化：使用位运算和查找表替代乘除法，消除条件分支
func (p *AStar) calH(node1, node2 *Node) int32 {
	// 优化：使用位运算计算绝对值（无分支）
	dx := node1.cube[0] - node2.cube[0]
	dx = (dx + (dx >> 31)) ^ (dx >> 31)

	dy := node1.cube[1] - node2.cube[1]
	dy = (dy + (dy >> 31)) ^ (dy >> 31)

	dz := node1.cube[2] - node2.cube[2]
	dz = (dz + (dz >> 31)) ^ (dz >> 31)

	// 优化：使用位移替代除法
	base := (dx + dy + dz) >> 1

	// 优化：使用查找表和位运算替代条件分支
	// 预计算启发式乘数：0=短距离×1.0, 1=中等距离×1.2, 2=长距离×1.1
	var multiplier int32 = 10 // 基础乘数
	if base > 50 {
		multiplier = 12 // 中等距离
		if base > 300 {
			multiplier = 11 // 长距离
		}
	}

	return (base * multiplier) / 10
}

// calG 计算从起点到当前节点的实际代价
// g = 父节点g值 + 移动代价 + 转向惩罚 + 视觉直线性惩罚
// 其中转向惩罚用于避免锯齿路径，视觉直线性惩罚用于优化路径外观
// 优化：简化转向惩罚计算逻辑，减少分支预测失败
func (p *AStar) calG(ctx *FindContext, node, parent *Node, cost int32) int32 {
	if parent == nil {
		return cost
	}

	// 获取祖父节点，用于判断方向变化
	grandpa := ctx.parentMap[parent]
	var pdx, pdy int32 // 父节点的移动方向
	if grandpa != nil {
		pdx = parent.x - grandpa.x
		pdy = parent.y - grandpa.y
	}

	// 当前移动方向
	dx := node.x - parent.x
	dy := node.y - parent.y

	// 优化转向惩罚计算，减少分支预测失败
	var turnPenalty int32 = 0
	if pdx != 0 || pdy != 0 { // 不是第一步
		// 使用异或运算快速检测方向变化
		directionChanged := ((dx ^ pdx) | (dy ^ pdy)) != 0

		if directionChanged {
			// 简化的转向判断逻辑
			if dx == 0 || pdx == 0 {
				turnPenalty = COST_TURN
			} else if node.isOdd {
				if dy != pdy && dy != pdy+1 {
					turnPenalty = COST_TURN
				}
			} else {
				if dy != pdy && dy != pdy-1 {
					turnPenalty = COST_TURN
				}
			}
		}
	}

	linearityPenalty := p.calLinearityPenalty(ctx, node, parent, grandpa)

	return ctx.gMap[parent] + cost + turnPenalty + linearityPenalty
}

// calLinearityPenalty 计算视觉直线性惩罚（使用预计算坐标）
// 目的：使路径在视觉上更加平滑，避免不必要的Y方向反向抖动
// 惩罚策略：
// 1. 惩罚y值低于起点/终点最小y的节点（防止路径向上偏移过多）
// 2. 惩罚视觉Y方向反向（在像素坐标下判断，符合实际显示效果）
// 3. 奖励同方向移动（鼓励保持方向一致性）
// 优化：完全使用整数运算，消除所有浮点运算
func (p *AStar) calLinearityPenalty(ctx *FindContext, node, parent, grandpa *Node) int32 {
	if parent == nil || ctx.start == nil || ctx.end == nil {
		return 0
	}

	var penalty int32 = 0

	// 惩罚1：如果y值低于起点和终点的最小y值
	// 防止路径向屏幕上方偏移过多
	// 优化：使用位移替代乘法
	if node.y < ctx.minY {
		penalty += COST_VISUAL_TURN << 1 // 乘以2
	}

	// 检查视觉Y方向反向（需要祖父节点来判断方向变化）
	if grandpa != nil {
		// 优化：使用预计算的整数坐标（×1000），完全消除浮点运算
		prevDyInt := parent.pixelYInt - grandpa.pixelYInt
		currDyInt := node.pixelYInt - parent.pixelYInt

		// Y方向反向（乘积为负表示方向相反）
		// 优化：使用异或运算检测符号变化，比乘法更快
		if (prevDyInt ^ currDyInt) < 0 {
			// 计算当前位置在整条路径中的进度比例（使用整数运算）
			totalDistInt := ctx.end.pixelXInt - ctx.start.pixelXInt
			nodeDistInt := node.pixelXInt - ctx.start.pixelXInt

			// 使用整数运算计算进度和距离中间的距离
			// progress * 1024 表示，512表示0.5（中间位置）
			// 优化：使用位移替代乘除法
			progressInt := int32(512) // 默认0.5
			if totalDistInt > 0 {
				// progress = nodeDist / totalDist，乘以1024
				progressInt = int32((nodeDistInt << 10) / totalDistInt)
			}

			// 离中间位置的距离（512表示中间）
			distFromMiddleInt := progressInt - 512
			if distFromMiddleInt < 0 {
				distFromMiddleInt = -distFromMiddleInt
			}

			// 惩罚公式：基础惩罚(30%) + 距离因子(140%)
			// Cost_Visual_Turn * (0.3 + distFromMiddle*1.4)
			// 优化：使用预计算和位运算
			// basePenalty = Cost_Visual_Turn * 3 / 10
			// distPenalty = Cost_Visual_Turn * distFromMiddleInt * 14 / 100
			basePenalty := int32((COST_VISUAL_TURN * 3) / 10)
			distPenalty := int32((COST_VISUAL_TURN * distFromMiddleInt * 14) / 100)
			penalty += basePenalty + distPenalty
		}

		// 同方向移动给予小奖励，鼓励保持方向一致性
		// 优化：使用位与运算检测同号
		if (prevDyInt & currDyInt) >= 0 {
			penalty -= 1
		}
	}

	return penalty
}

// Print 可视化打印路径
// 用于调试，将路径在地图上以ASCII图形展示
// 图例：◉ 起点, ◦ 路径点, ● 终点
func (p *AStar) Print(list []*Node) {
	if len(list) < 1 {
		return
	}

	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.Min.X {
		// 偶数行缩进，模拟六边形交错布局
		if x%2 == 0 {
			mapText += " "
		}
		for y := range p.config.Max.Y {
			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)
}

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) 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
}