From 100268c10e561628e30361a05f4e715286bfc407 Mon Sep 17 00:00:00 2001 From: kercylan98 Date: Tue, 23 May 2023 12:07:36 +0800 Subject: [PATCH] =?UTF-8?q?=E4=B8=89=E7=BB=B4A*=E5=AF=BB=E8=B7=AF=E5=AE=9E?= =?UTF-8?q?=E7=8E=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- utils/astar/astar.go | 99 +++++++++++++++++++++++++++++++++++++++ utils/astar/astar_test.go | 77 ++++++++++++++++++++++++++++++ utils/astar/node3d.go | 42 +++++++++++++++++ utils/astar/point3d.go | 5 ++ 4 files changed, 223 insertions(+) create mode 100644 utils/astar/astar.go create mode 100644 utils/astar/astar_test.go create mode 100644 utils/astar/node3d.go create mode 100644 utils/astar/point3d.go diff --git a/utils/astar/astar.go b/utils/astar/astar.go new file mode 100644 index 0000000..3a4b599 --- /dev/null +++ b/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 +} diff --git a/utils/astar/astar_test.go b/utils/astar/astar_test.go new file mode 100644 index 0000000..272abc8 --- /dev/null +++ b/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() + } +} diff --git a/utils/astar/node3d.go b/utils/astar/node3d.go new file mode 100644 index 0000000..a95b0f5 --- /dev/null +++ b/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 +} diff --git a/utils/astar/point3d.go b/utils/astar/point3d.go new file mode 100644 index 0000000..df0afbe --- /dev/null +++ b/utils/astar/point3d.go @@ -0,0 +1,5 @@ +package astar + +type Point3D struct { + X, Y, Z int +}