86 lines
1.8 KiB
Go
86 lines
1.8 KiB
Go
|
package sauce
|
||
|
|
|
||
|
|
type Node struct {
|
||
|
|
parent *Node
|
||
|
|
x, y int
|
||
|
|
f, g, h int
|
||
|
|
}
|
||
|
|
|
||
|
|
func FindPath(w *World, fromX, fromY int, toX, toY int) []Node {
|
||
|
|
openList := make([]*Node, 0)
|
||
|
|
closedList := make([]*Node, 0)
|
||
|
|
|
||
|
|
startNode := &Node{x: fromX, y: fromY}
|
||
|
|
endNode := &Node{x: toX, y: toY}
|
||
|
|
|
||
|
|
openList = append(openList, startNode)
|
||
|
|
|
||
|
|
for len(openList) > 0 {
|
||
|
|
var cnode *Node
|
||
|
|
var cindex int
|
||
|
|
for i, node := range openList {
|
||
|
|
if cnode == nil || node.f < cnode.f {
|
||
|
|
cnode = node
|
||
|
|
cindex = i
|
||
|
|
}
|
||
|
|
}
|
||
|
|
openList = append(openList[:cindex], openList[cindex+1:]...)
|
||
|
|
closedList = append(closedList, cnode)
|
||
|
|
|
||
|
|
if cnode.x == endNode.x && cnode.y == endNode.y {
|
||
|
|
path := make([]Node, 0)
|
||
|
|
for cnode != nil {
|
||
|
|
path = append(path, *cnode)
|
||
|
|
cnode = cnode.parent
|
||
|
|
}
|
||
|
|
return path
|
||
|
|
}
|
||
|
|
|
||
|
|
var neighbors []Node
|
||
|
|
for x := -1; x <= 1; x++ {
|
||
|
|
for y := -1; y <= 1; y++ {
|
||
|
|
if x == 0 && y == 0 {
|
||
|
|
continue
|
||
|
|
}
|
||
|
|
if x != 0 && y != 0 {
|
||
|
|
continue
|
||
|
|
}
|
||
|
|
if cell := w.At(cnode.x+x, cnode.y+y); cell != nil {
|
||
|
|
neighbors = append(neighbors, Node{x: cnode.x + x, y: cnode.y + y, parent: cnode})
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
for _, neighbor := range neighbors {
|
||
|
|
exists := false
|
||
|
|
for _, node := range closedList {
|
||
|
|
if node.x == neighbor.x && node.y == neighbor.y {
|
||
|
|
exists = true
|
||
|
|
break
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if exists {
|
||
|
|
continue
|
||
|
|
}
|
||
|
|
|
||
|
|
exists = false
|
||
|
|
for _, node := range openList {
|
||
|
|
if node.x == neighbor.x && node.y == neighbor.y {
|
||
|
|
exists = true
|
||
|
|
break
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if exists {
|
||
|
|
continue
|
||
|
|
}
|
||
|
|
|
||
|
|
neighbor.g = cnode.g + 1 // this value potentially should be a float and modified if a diagonal.
|
||
|
|
neighbor.h = (toX - neighbor.x) + (toY - neighbor.y) // Maybe should be calculated differently.
|
||
|
|
neighbor.f = neighbor.g + neighbor.h
|
||
|
|
|
||
|
|
openList = append(openList, &neighbor)
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return nil
|
||
|
|
}
|