三维A*寻路实现

utils/astar/astar.go

@@ -0,0 +1,99 @@
+package astar
+
+import (
+	"container/heap"
+	"fmt"
+	"math"
+)
+
+// Pathfinding3D 寻路
+//   - grid: X、Y、Z
+func Pathfinding3D(grid [][][]int, start, target Point3D, collisionRange int) ([]Point3D, error) {
+	path, err := aStar3D(grid, start, target, collisionRange)
+	if err != nil {
+		return nil, err
+	}
+	return path, nil
+}
+
+func aStar3D(grid [][][]int, start, goal Point3D, collisionRange int) ([]Point3D, error) {
+	startNode := &Node3D{point: start, g: 0, h: heuristic(start, goal)}
+	startNode.f = startNode.g + startNode.h
+	openList := make(Node3DList, 0)
+	heap.Init(&openList)
+	heap.Push(&openList, startNode)
+	closedList := make(map[Point3D]*Node3D)
+
+	for openList.Len() > 0 {
+		current := heap.Pop(&openList).(*Node3D)
+		if current.point == goal {
+			path := make([]Point3D, 0)
+			for current != nil {
+				path = append([]Point3D{current.point}, path...)
+				current = current.parent
+			}
+			return path, nil
+		}
+
+		closedList[current.point] = current
+		for _, neighborPoint := range neighbors(current.point, grid, collisionRange) {
+			if _, ok := closedList[neighborPoint]; ok {
+				continue
+			}
+			tentativeG := current.g + 1
+			neighborNode := &Node3D{point: neighborPoint, parent: current, g: tentativeG, h: heuristic(neighborPoint, goal)}
+			neighborNode.f = neighborNode.g + neighborNode.h
+			for _, openNode := range openList {
+				if openNode.point == neighborPoint && tentativeG >= openNode.g {
+					continue
+				}
+			}
+			heap.Push(&openList, neighborNode)
+		}
+	}
+	return nil, fmt.Errorf("no path found")
+}
+
+func heuristic(a, b Point3D) float64 {
+	return math.Abs(float64(a.X-b.X)) + math.Abs(float64(a.Y-b.Y)) + math.Abs(float64(a.Z-b.Z))
+}
+
+func inBounds(point Point3D, grid [][][]int) bool {
+	return point.X >= 0 && point.X < len(grid) && point.Y >= 0 && point.Y < len(grid[0]) && point.Z >= 0 && point.Z < len(grid[0][0])
+}
+
+func passable(point Point3D, grid [][][]int, collisionRange int) bool {
+	for x := -collisionRange; x <= collisionRange; x++ {
+		for y := -collisionRange; y <= collisionRange; y++ {
+			for z := -collisionRange; z <= collisionRange; z++ {
+				newPoint := Point3D{point.X + x, point.Y + y, point.Z + z}
+				if inBounds(newPoint, grid) && grid[newPoint.X][newPoint.Y][newPoint.Z] == 1 {
+					return false
+				}
+			}
+		}
+	}
+	return true
+}
+
+func neighbors(point Point3D, grid [][][]int, collisionRange int) []Point3D {
+	neighborPoints := []Point3D{
+		{point.X - 1, point.Y, point.Z}, {point.X + 1, point.Y, point.Z},
+		{point.X, point.Y - 1, point.Z}, {point.X, point.Y + 1, point.Z},
+		{point.X, point.Y, point.Z - 1}, {point.X, point.Y, point.Z + 1},
+	}
+	result := make([]Point3D, 0)
+	for _, neighbor := range neighborPoints {
+		if inBounds(neighbor, grid) && passable(neighbor, grid, collisionRange) {
+			result = append(result, neighbor)
+		}
+	}
+	return result
+}
+
+func abs(x int) int {
+	if x < 0 {
+		return -x
+	}
+	return x
+}

utils/astar/astar_test.go

@@ -0,0 +1,77 @@
+package astar
+
+import (
+	"fmt"
+	"testing"
+)
+
+func TestPathfinding3D(t *testing.T) {
+	grid := [][][]int{
+		{
+			{0, 0, 0, 0, 0},
+			{0, 0, 0, 0, 0},
+			{0, 0, 0, 0, 0},
+			{0, 0, 0, 0, 0},
+			{0, 0, 0, 0, 0},
+		},
+		{
+			{0, 0, 0, 0, 0},
+			{0, 1, 1, 1, 0},
+			{0, 1, 0, 1, 0},
+			{0, 1, 1, 1, 0},
+			{0, 0, 0, 0, 0},
+		},
+		{
+			{1, 0, 0, 0, 0},
+			{0, 1, 0, 1, 0},
+			{0, 0, 0, 0, 0},
+			{0, 1, 0, 1, 0},
+			{0, 0, 0, 0, 0},
+		},
+		{
+			{0, 0, 0, 0, 0},
+			{0, 1, 1, 1, 0},
+			{0, 1, 0, 1, 0},
+			{0, 1, 1, 1, 0},
+			{0, 0, 0, 0, 0},
+		},
+		{
+			{0, 0, 0, 0, 0},
+			{0, 0, 0, 0, 0},
+			{0, 0, 0, 0, 0},
+			{0, 0, 0, 0, 0},
+			{0, 0, 0, 0, 0},
+		},
+	}
+
+	start := Point3D{2, 1, 0}
+	target := Point3D{2, 2, 2}
+	collisionRange := 0
+
+	path, err := Pathfinding3D(grid, start, target, collisionRange)
+	if err != nil {
+		panic(err)
+	}
+
+	fmt.Println(path)
+	printGrid(grid, start)
+}
+
+func printGrid(grid [][][]int, start Point3D) {
+	for z := 0; z < len(grid); z++ {
+		fmt.Printf("z = %d\n", z)
+		for y := 0; y < len(grid[z]); y++ {
+			for x := 0; x < len(grid[z][y]); x++ {
+				if x == start.X && y == start.Y && z == start.Z {
+					fmt.Print("S ")
+				} else if grid[z][y][x] == 0 {
+					fmt.Print(". ")
+				} else {
+					fmt.Print("# ")
+				}
+			}
+			fmt.Println()
+		}
+		fmt.Println()
+	}
+}

utils/astar/node3d.go

@@ -0,0 +1,42 @@
+package astar
+
+type Node3D struct {
+	point   Point3D
+	parent  *Node3D
+	g, h, f float64
+	index   int
+}
+
+type Node3DList []*Node3D
+
+func (slf *Node3DList) Len() int {
+	return len(*slf)
+}
+
+func (slf *Node3DList) Less(i, j int) bool {
+	nl := *slf
+	return nl[i].f < nl[j].f
+}
+
+func (slf *Node3DList) Swap(i, j int) {
+	nl := *slf
+	nl[i], nl[j] = nl[j], nl[i]
+	nl[i].index = i
+	nl[j].index = j
+}
+
+func (slf *Node3DList) Push(x interface{}) {
+	n := len(*slf)
+	node := x.(*Node3D)
+	node.index = n
+	*slf = append(*slf, node)
+}
+
+func (slf *Node3DList) Pop() interface{} {
+	old := *slf
+	n := len(old)
+	node := old[n-1]
+	node.index = -1
+	*slf = old[0 : n-1]
+	return node
+}

utils/astar/point3d.go

@@ -0,0 +1,5 @@
+package astar
+
+type Point3D struct {
+	X, Y, Z int
+}