gridpathingtest/sauce/astar.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
}