last_light/engine/pathfinding.go

package engine

import (
	"slices"
)

type pathNode struct {
	pos Position

	parent *pathNode

	g int // distance between current node and start node
	h int // heuristic - squared distance from current node to end node
	f int // total cost of this node
}

func FindPath(from Position, to Position, isPassable func(x, y int) bool) *Path {
	var openList = make([]*pathNode, 0)
	var closedList = make([]*pathNode, 0)

	openList = append(openList, &pathNode{
		pos:    from,
		parent: nil,
		g:      0,
		h:      0,
		f:      0,
	})

	var lastNode *pathNode

	for {

		if len(openList) == 0 {
			break
		}

		// find node in open list with lowest f value, remove it from open and move it to closed
		currentIndex := 0
		currentNode := openList[currentIndex]

		for i, node := range openList {
			if node.f < currentNode.f {
				currentNode = node
				currentIndex = i
			}
		}

		if currentNode.pos.Equals(to) {
			lastNode = currentNode
			break // We have reached the goal
		}

		openList = slices.Delete(openList, currentIndex, currentIndex+1)
		closedList = append(closedList, currentNode)

		// use adjacent nodes as children
		children := []*pathNode{
			{
				pos:    currentNode.pos.WithOffset(1, 0),
				parent: currentNode,
			},
			{
				pos:    currentNode.pos.WithOffset(-1, 0),
				parent: currentNode,
			},
			{
				pos:    currentNode.pos.WithOffset(0, 1),
				parent: currentNode,
			},
			{
				pos:    currentNode.pos.WithOffset(0, -1),
				parent: currentNode,
			},
		}

		for _, child := range children {
			// If the child is impassable, skip it
			if !isPassable(child.pos.XY()) {
				continue
			}

			// If child is already contained in closedList, skip it
			if slices.ContainsFunc(closedList, func(el *pathNode) bool { return el.pos.Equals(child.pos) }) {
				continue
			}

			child.g = currentNode.g + 1
			child.h = to.DistanceSquared(child.pos)
			child.f = child.g + child.h

			// If child is already contained in openList, and has lower g
			if slices.ContainsFunc(openList, func(el *pathNode) bool { return el.pos.Equals(child.pos) && child.g > el.g }) {
				continue
			}

			openList = append(openList, child)
		}

	}

	if lastNode == nil {
		return nil
	}

	node := lastNode
	path := make([]Position, 0)

	for {
		path = append(path, node.pos)

		if node.parent == nil {
			break
		}

		node = node.parent
	}

	slices.Reverse(path)

	return CreatePath(
		from, to,
		path,
	)
}