golang实现障碍、转弯最少的A*寻路

目录

• 目标：
• 要点：
• 源码：

目标：

• 优先寻找无障碍的路径
• 目标不可达时，寻找障碍最少的路径
• 路径长度相等时，优先转弯最少的路径
• 多个目标点时，根据以上要求到达其中一个目标点即可

要点：

• 最优格子的选取，先对open list排序，然后从open list中出队

源码：

package main

import (
	"errors"
	"fmt"
	"math"
	"sort"
)

type Node struct {
	Parent *Node
	Pos    []int32
	GDir   []int32
	G      float32
	H      float32
	F      float32
	B      int // 障碍Block
	T      int // 转弯turn
}

func (node *Node) String() string {
	return fmt.Sprintf("%#v", node)
}

// 优先障碍最少，其次路径最短，再次转弯最少
func (best *Node) IsBetterThan(node *Node) bool {
	if best.B < node.B {
		return true
	}
	if best.B > node.B {
		return false
	}
	if best.F < node.F {
		return true
	}
	if best.F > node.F {
		return false
	}
	if best.T < node.T {
		return true
	}
	if best.T > node.T {
		return false
	}
	return false
}

type NodeList []*Node

func (nl NodeList) Len() int      { return len(nl) }
func (nl NodeList) Swap(i, j int) { nl[i], nl[j] = nl[j], nl[i] }
func (nl NodeList) Less(i, j int) bool {
	best, node := nl[i], nl[j]
	return best.IsBetterThan(node)
}

func (nl NodeList) String() string {
	s := "*******NodeList********\n"
	for i, node := range nl {
		s = fmt.Sprintf("%s%d = %s\n", s, i, node.String())
	}
	return s
}

func (nl NodeList) NodeIdx(node *Node) int {
	for i, n := range nl {
		if n.Pos[0] == node.Pos[0] && n.Pos[1] == node.Pos[1] {
			return i
		}
	}
	return -1
}

type AStar struct {
	W        int32
	H        int32
	SPos     []int32
	EPos     []int32
	Open     NodeList // 使用list，起点，终点相同时每次的路径都一样，换成map时，每次的路径可能不一样。
	CloseMap map[int32]struct{}
}

const (
	NODE_OPEN    int32 = iota // 可通行
	NODE_BARRIER              // 障碍，无法到达目的地时，返回障碍最少的路径
	NODE_NO_PASS              // 不可通行，边界等不能通行
)

func (a *AStar) newNode(parent *Node, x, y int32, dir []int32) *Node {
	state := a.GetNodeState(x, y)
	if state != NODE_OPEN && state != NODE_BARRIER {
		a.addClose(&Node{Pos: []int32{x, y}})
		return nil
	}
	b, t := 0, 0
	if parent != nil {
		b = parent.B
		if dir[0] == parent.GDir[0] && dir[1] == parent.GDir[1] {
			t = parent.T
		} else {
			t = parent.T + 1
		}
	}
	if state == NODE_BARRIER {
		b++
	}
	pos := []int32{x, y}
	g := a.getG(pos, parent)
	h := a.getH(pos, a.EPos)
	return &Node{
		Parent: parent,
		Pos:    pos,
		G:      g,
		H:      h,
		F:      g + h,
		B:      b,
		T:      t,
		GDir:   dir,
	}
}

func (a *AStar) GetNodeState(x, y int32) int32 {
	if x < 0 || x >= a.W || y < 0 || y >= a.H {
		return NODE_NO_PASS
	}
	//fmt.Println("GetNodeState", x, y, GMap[x][y])
	return GMap[x][y]
}

// 计算g值；直走=1；斜走=1.4
func (a *AStar) getG(pos1 []int32, parent *Node) float32 {
	if parent == nil {
		return 0
	}
	pos2 := parent.Pos
	g := parent.G
	if pos1[0] == pos2[0] {
		return g + float32(math.Abs(float64(pos1[1]-pos2[1])))
	} else if pos1[1] == pos2[1] {
		return g + float32(math.Abs(float64(pos1[0]-pos2[0])))
	} else {
		return g + float32(math.Abs(float64(pos1[0]-pos2[0])*1.4))
	}
}

// 计算h值
func (a *AStar) getH(pos1, pos2 []int32) float32 {
	return float32(math.Abs(float64(pos1[0]-pos2[0])) + math.Abs(float64(pos1[1]-pos2[1])))
}

func (a *AStar) addOpen(node *Node) {
	//idx := a.getNodeIdxInOpen(node)
	idx := a.Open.NodeIdx(node)
	//fmt.Println(a.Open)
	//fmt.Println("addOpen:", idx, "node:", node, "patrnt ", node.Parent)
	if idx >= 0 {
		n := a.Open[idx]
		if node.IsBetterThan(n) {
			a.Open[idx] = node
		}
	} else {
		a.Open = append(a.Open, node)
	}
}

// x, y 对应的node在open中的idx
func (a *AStar) getNodeIdxInOpen(node *Node) int {
	x, y := node.Pos[0], node.Pos[1]
	l := len(a.Open)
	if l == 0 {
		return -1
	}
	idx := sort.Search(len(a.Open),
		func(i int) bool {
			pos := a.Open[i].Pos
			return pos[0] == x && pos[1] == y
		})
	// sort.Search 对已排序的list使用二分查找，
	// sort.Search 的坑，未找到返回的值等于list长度
	if idx == l {
		return -1
	}
	return idx
}

// 从open中出栈，即最优的格子
func (a *AStar) getMinNode() *Node {
	sort.Sort(a.Open)
	node := a.Open[0]
	a.Open = a.Open[1:]
	return node
}

// 添加到close中,
func (a *AStar) addClose(n *Node) {
	x, y := n.Pos[0], n.Pos[1]
	key := x*a.W*10 + y
	a.CloseMap[key] = struct{}{}
}

// 判断是否在close中
func (a *AStar) isInClose(x, y int32) (ok bool) {
	key := x*a.W*10 + y
	_, ok = a.CloseMap[key]
	return
}

// 拓展周边方向的node
func (a *AStar) extendNeighbours(c *Node) {
	for _, dir := range GDir {
		x := c.Pos[0] + dir[0]
		y := c.Pos[1] + dir[1]
		if a.isInClose(x, y) {
			continue
		}
		node := a.newNode(c, x, y, dir)
		if node == nil {
			continue
		}
		a.addOpen(node)
	}
}

func (a *AStar) isTarget(n *Node, ePos []int32) bool {
	if n == nil {
		return false
	}
	if n.Pos[0] == ePos[0] && n.Pos[1] == ePos[1] {
		return true
	}
	return false
}

// 从结束点回溯到开始点，开始点的parent == None
func (a *AStar) makePath(p *Node) [][]int32 {
	path := make([][]int32, 0)
	for p != nil {
		path = append([][]int32{p.Pos}, path[:]...)
		p = p.Parent
	}
	fmt.Println("********makePath:", path)
	return path
}

// 路径查找主函数
func (a *AStar) findPath(sPos, ePos []int32) (path [][]int32, block, turn int, err error) {
	state := a.GetNodeState(sPos[0], sPos[1])
	if state != NODE_OPEN && state != NODE_BARRIER {
		err = errors.New(fmt.Sprintf("spos state is %d", state))
		return
	}

	estate := a.GetNodeState(ePos[0], ePos[1])
	if estate != NODE_OPEN && estate != NODE_BARRIER {
		err = errors.New(fmt.Sprintf("ePos state is %d", estate))
		return
	}

	a.SPos, a.EPos = sPos, ePos
	// 构建开始节点
	s := a.newNode(nil, sPos[0], sPos[1], []int32{0, 0})
	a.addOpen(s)
	for {
		if len(a.Open) <= 0 {
			err = errors.New("not find Open list is nil")
			return
		}
		p := a.getMinNode()
		//fmt.Println(fmt.Sprintf("---min p = %#v", p))
		if a.isTarget(p, ePos) {
			path = a.makePath(p)
			block, turn = p.B, p.T
			return
		}
		a.extendNeighbours(p)
		a.addClose(p)
	}
	return
}

// 查询过的所有格子
func (a *AStar) getSearched() []int32 {
	l := make([]int32, 0)
	for _, i := range a.Open {
		if i != nil {
			l = append(l, i.Pos[0], i.Pos[1])
		}
	}
	for k, _ := range a.CloseMap {
		x := k / a.W * 10
		y := k % a.W * 10
		l = append(l, x, y)
	}
	return l
}

// 清空open和close
func (a *AStar) clean() {
	a.Open = make(NodeList, 0)
	a.CloseMap = make(map[int32]struct{})
}

func NewAstar() *AStar {
	a := &AStar{
		W:        int32(len(GMap)),
		H:        int32(len(GMap[0])),
		Open:     make(NodeList, 0),
		CloseMap: make(map[int32]struct{}),
	}
	fmt.Println("NewAstar", a.W, a.H)
	return a
}

// 终点不唯一时找到最优的路径
func FindPathIgnoreBlock(sPos []int32, ePos [][]int32) {
	a := NewAstar()
	if a == nil {
		return
	}
	var path [][]int32
	var block, turn int
	for _, e := range ePos {
		a.clean()
		p, b, t, err := a.findPath(sPos, e)
		fmt.Println("FindPathIgnoreBlock:",
			fmt.Sprint("sPos", sPos),
			fmt.Sprint("ePos", e),
			fmt.Sprint("path", p),
			fmt.Sprint("Block", b),
			fmt.Sprint("turn", t),
			fmt.Sprint("err", err))
		if err != nil {
			continue
		}
		if path == nil {
			path, block, turn = p, b, t
			continue
		}
		stepNum, newStep := len(path), len(p)
		fmt.Println("stepNum", stepNum, "newStep", newStep)
		if stepNum < newStep {
			continue
		}
		if stepNum > newStep {
			path, block, turn = p, b, t
			continue
		}
		if block < b {
			continue
		}
		if block > b {
			path, block, turn = p, b, t
			continue
		}

		if turn < t {
			continue
		}

		if turn > b {
			path, block, turn = p, b, t
			continue
		}
	}
	fmt.Println("********end, FindPathIgnoreBlock", path, block, turn)
}

var (
	GDir [][]int32 //方向
	GMap [][]int32 //地图
)

func init() {
	GDir = [][]int32{{1, 0}, {0, 1}, {0, -1}, {-1, 0}}
	GMap = [][]int32{
		{0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0},
		{1, 1, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0},
		{0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0},
		{0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0},
		{0, 1, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0},
		{0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0},
		{2, 2, 2, 2, 2, 1, 2, 2, 1, 2, 2, 2, 2, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0},
		{0, 0, 0, 1, 0, 1, 0, 2, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0},
		{0, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0},
		{0, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0},
		{0, 0, 0, 1, 2, 1, 0, 0, 1, 0, 2, 0, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0},
		{0, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0},
		{0, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 2, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0},
		{0, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 2, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0},
		{0, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 2, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0},
		{0, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 2, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0},
		{0, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 2, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0},
		{0, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 2, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0},
		{0, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 2, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0},
		{0, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 2, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0},
		{0, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 2, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0},
		{0, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 2, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0},
		{0, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 2, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0},
	}
	fmt.Println("----init:", len(GMap[0]), len(GMap), NODE_OPEN, NODE_BARRIER, NODE_NO_PASS)
}

func main() {
	FindPathIgnoreBlock([]int32{3, 2}, [][]int32{{5, 3}})
}