101 lines
2.3 KiB
Go
101 lines
2.3 KiB
Go
package sauce
|
|
|
|
import (
|
|
"math"
|
|
)
|
|
|
|
type Coord struct {
|
|
X, Y int16
|
|
}
|
|
|
|
type Node struct {
|
|
Coord
|
|
parent *Node
|
|
g int
|
|
f int
|
|
d int
|
|
}
|
|
|
|
type Nodes map[Coord]*Node
|
|
|
|
func FindPath(w *World, fromX, fromY int, toX, toY int) ([]Node, bool) {
|
|
var open []*Node
|
|
visited := make(Nodes)
|
|
|
|
open = append(open, &Node{Coord: Coord{int16(fromX), int16(fromY)}})
|
|
|
|
for len(open) > 0 {
|
|
var current *Node
|
|
var currentI int
|
|
// Set current to lowest f in open
|
|
for i, n := range open {
|
|
if current == nil || n.f < current.f {
|
|
current = n
|
|
currentI = i
|
|
}
|
|
}
|
|
|
|
// Return reconstructed path if current is the destination.
|
|
if current.X == int16(toX) && current.Y == int16(toY) {
|
|
var path []Node
|
|
for current.parent != nil {
|
|
path = append(path, *current)
|
|
current = current.parent
|
|
}
|
|
return path, true
|
|
}
|
|
// Remove current from the open set.
|
|
open = append(open[:currentI], open[currentI+1:]...)
|
|
|
|
// Get neighbors.
|
|
var neighbors []*Node
|
|
for x := -1; x <= 1; x++ {
|
|
for y := -1; y <= 1; y++ {
|
|
if x == 0 && y == 0 {
|
|
continue
|
|
}
|
|
coord := Coord{current.X + int16(x), current.Y + int16(y)}
|
|
if cell := w.At(int(coord.X), int(coord.Y)); cell == nil || cell.Hueristic == 255 {
|
|
continue
|
|
}
|
|
neighbors = append(neighbors, &Node{Coord: coord, parent: current, g: math.MaxUint16, f: math.MaxUint16})
|
|
}
|
|
}
|
|
|
|
// Iterate over neighbors.
|
|
for _, neighbor := range neighbors {
|
|
tentativeG := current.g + 1 + int(w.At(int(neighbor.X), int(neighbor.Y)).Hueristic)
|
|
distance := int(math.Abs(float64(neighbor.X)-float64(toX)) + math.Abs(float64(neighbor.Y)-float64(toY)))
|
|
tentativeF := tentativeG + distance
|
|
|
|
var node *Node
|
|
if node = visited[neighbor.Coord]; node == nil || tentativeF < node.f {
|
|
if node == nil {
|
|
node = neighbor
|
|
}
|
|
node.g = tentativeG
|
|
node.f = tentativeF
|
|
node.d = distance
|
|
visited[node.Coord] = node
|
|
open = append(open, node)
|
|
}
|
|
}
|
|
}
|
|
|
|
var current *Node
|
|
// Set current to lowest f in open
|
|
for _, n := range visited {
|
|
if current == nil || n.d < current.d {
|
|
current = n
|
|
}
|
|
}
|
|
|
|
// Return reconstructed path if current is the destination.
|
|
var path []Node
|
|
for current != nil && current.parent != nil {
|
|
path = append(path, *current)
|
|
current = current.parent
|
|
}
|
|
return path, false
|
|
}
|