115 lines
2.4 KiB
Go
115 lines
2.4 KiB
Go
package main
|
|
|
|
import "fmt"
|
|
|
|
type Node struct {
|
|
Tile Tile
|
|
Parent *Node
|
|
G, H, F float32
|
|
}
|
|
|
|
func FindPath(start, end Tile) []Tile {
|
|
openList := []*Node{}
|
|
closedList := make(map[[2]int]bool)
|
|
|
|
startNode := &Node{Tile: start, G: 0, H: heuristic(start, end)}
|
|
startNode.F = startNode.G + startNode.H
|
|
openList = append(openList, startNode)
|
|
|
|
for len(openList) > 0 {
|
|
// Find node with lowest F
|
|
current := openList[0]
|
|
currentIndex := 0
|
|
for i, node := range openList {
|
|
if node.F < current.F {
|
|
current = node
|
|
currentIndex = i
|
|
}
|
|
}
|
|
|
|
// Move current to closed list
|
|
openList = append(openList[:currentIndex], openList[currentIndex+1:]...)
|
|
closedList[[2]int{current.Tile.X, current.Tile.Y}] = true
|
|
|
|
// Check if reached the end
|
|
if current.Tile.X == end.X && current.Tile.Y == end.Y {
|
|
path := []Tile{}
|
|
node := current
|
|
for node != nil {
|
|
path = append([]Tile{node.Tile}, path...)
|
|
node = node.Parent
|
|
}
|
|
fmt.Printf("Path found: %v\n", path)
|
|
return path
|
|
}
|
|
|
|
// Generate neighbors
|
|
neighbors := GetNeighbors(current.Tile)
|
|
for _, neighbor := range neighbors {
|
|
if !neighbor.Walkable || closedList[[2]int{neighbor.X, neighbor.Y}] {
|
|
continue
|
|
}
|
|
|
|
tentativeG := current.G + distance(current.Tile, neighbor)
|
|
inOpen := false
|
|
var existingNode *Node
|
|
for _, node := range openList {
|
|
if node.Tile.X == neighbor.X && node.Tile.Y == neighbor.Y {
|
|
existingNode = node
|
|
inOpen = true
|
|
break
|
|
}
|
|
}
|
|
|
|
if !inOpen || tentativeG < existingNode.G {
|
|
newNode := &Node{
|
|
Tile: neighbor,
|
|
Parent: current,
|
|
G: tentativeG,
|
|
H: heuristic(neighbor, end),
|
|
}
|
|
newNode.F = newNode.G + newNode.H
|
|
if !inOpen {
|
|
openList = append(openList, newNode)
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// No path found
|
|
fmt.Println("No path found")
|
|
return nil
|
|
}
|
|
|
|
func heuristic(a, b Tile) float32 {
|
|
return float32(abs(a.X-b.X) + abs(a.Y-b.Y))
|
|
}
|
|
|
|
func distance(a, b Tile) float32 {
|
|
_ = a
|
|
_ = b
|
|
return 1.0 //uniform cost for now
|
|
}
|
|
|
|
func GetNeighbors(tile Tile) []Tile {
|
|
directions := [][2]int{
|
|
{1, 0}, {-1, 0}, {0, 1}, {0, -1},
|
|
{1, 1}, {-1, -1}, {1, -1}, {-1, 1},
|
|
}
|
|
neighbors := []Tile{}
|
|
for _, dir := range directions {
|
|
nx, ny := tile.X+dir[0], tile.Y+dir[1]
|
|
if nx >= 0 && nx < MapWidth && ny >= 0 && ny < MapHeight {
|
|
neighbors = append(neighbors, mapGrid[nx][ny])
|
|
}
|
|
}
|
|
return neighbors
|
|
}
|
|
|
|
func abs(x int) int {
|
|
if x < 0 {
|
|
return -x
|
|
}
|
|
return x
|
|
}
|