The A* (astar, a-star) pathfinding algorithm

main.go

package main

import (
	"fmt"
	"strconv"
	"strings"

	qu "github.com/x1m3/priorityQueue"
)

type Node struct {
	ID        string
	X         int
	Y         int
	H, G, F   int
	Edges     map[string]int
	Traversed bool
	Path      []string
}

func (node *Node) HigherPriorityThan(value qu.Interface) bool {
	otherNode := value.(*Node)
	return node.F < otherNode.F
}

func Abs(arg int) int {
	if arg < 0 {
		return -1 * arg
	}

	return arg
}

func ManhattanDistance(x1, y1, x2, y2 int) int {
	return Abs(x1-x2) + Abs(y1-y2)
}

func main() {
	tileMap := [][]string{
		{" ", " ", " ", " ", " ", " ", " ", " "},
		{" ", " ", " ", " ", " ", " ", " ", " "},
		{" ", " ", "*", "*", "*", " ", " ", " "},
		{" ", " ", " ", " ", "*", " ", " ", " "},
		{" ", " ", " ", " ", "*", " ", " ", " "},
		{" ", "S", " ", " ", "*", " ", " ", " "},
		{" ", " ", " ", " ", "*", " ", " ", " "},
		{" ", " ", " ", " ", "*", " ", " ", " "},
		{" ", " ", "*", "*", "*", " ", " ", " "},
		{" ", " ", " ", " ", " ", "F", " ", " "},
		{" ", " ", " ", " ", " ", " ", " ", " "},
	}
	graph := map[string]*Node{}
	var start, finish *Node

	// Create nodes.
	for i := 0; i < len(tileMap); i++ {
		for j := 0; j < len(tileMap[0]); j++ {
			id := strconv.Itoa(i) + "_" + strconv.Itoa(j)
			graph[id] = &Node{
				ID:    id,
				X:     i,
				Y:     j,
				Edges: map[string]int{},
			}

			if tileMap[i][j] == "S" {
				start = graph[id]
			}

			if tileMap[i][j] == "F" {
				finish = graph[id]
			}
		}
	}

	// Create edges.
	for i := 0; i < len(tileMap); i++ {
		for j := 0; j < len(tileMap[0]); j++ {
			currentID := strconv.Itoa(i) + "_" + strconv.Itoa(j)
			current := graph[currentID]

			if j != 0 {
				neighbourID := strconv.Itoa(i) + "_" + strconv.Itoa(j-1)
				current.Edges[neighbourID] = 1
			}

			if i != 0 {
				neighbourID := strconv.Itoa(i-1) + "_" + strconv.Itoa(j)
				current.Edges[neighbourID] = 1
			}

			if i != len(tileMap)-1 {
				neighbourID := strconv.Itoa(i+1) + "_" + strconv.Itoa(j)
				current.Edges[neighbourID] = 1
			}

			if j != len(tileMap[0])-1 {
				neighbourID := strconv.Itoa(i) + "_" + strconv.Itoa(j+1)
				current.Edges[neighbourID] = 1
			}
		}
	}

	// Create obstacles.
	for i := 0; i < len(tileMap); i++ {
		for j := 0; j < len(tileMap[0]); j++ {
			if tileMap[i][j] == "*" {
				currentID := strconv.Itoa(i) + "_" + strconv.Itoa(j)

				if i != 0 {
					neighbourID := strconv.Itoa(i-1) + "_" + strconv.Itoa(j)

					if neighbour, ok := graph[neighbourID]; ok {
						delete(neighbour.Edges, currentID)
					}
				}

				if j != 0 {
					neighbourID := strconv.Itoa(i) + "_" + strconv.Itoa(j-1)

					if neighbour, ok := graph[neighbourID]; ok {
						delete(neighbour.Edges, currentID)
					}
				}

				if i != len(tileMap)-1 {
					neighbourID := strconv.Itoa(i+1) + "_" + strconv.Itoa(j)

					if neighbour, ok := graph[neighbourID]; ok {
						delete(neighbour.Edges, currentID)
					}
				}

				if j != len(tileMap)-1 {
					neighbourID := strconv.Itoa(i) + "_" + strconv.Itoa(j+1)

					if neighbour, ok := graph[neighbourID]; ok {
						delete(neighbour.Edges, currentID)
					}
				}

				delete(graph, currentID)
			}
		}
	}

	start.G = 0
	start.H = ManhattanDistance(
		start.X, start.Y, finish.X, finish.Y)
	start.F = start.G + start.H
	start.Traversed = true
	start.Path = []string{start.ID}

	queue := qu.New()
	queue.Push(start)

	for vertex := queue.Pop(); vertex != nil; vertex = queue.Pop() {
		node := vertex.(*Node)

		if node == finish {
			break
		}

		for neighbourID, weight := range node.Edges {
			neighbourNode := graph[neighbourID]

			if neighbourNode.Traversed {
				continue
			}

			// g(x)
			neighbourNode.G = node.G + weight
			// h(x)
			neighbourNode.H = ManhattanDistance(
				neighbourNode.X, neighbourNode.Y, finish.X, finish.Y)
			// f(x) = g(x) + h(x)
			neighbourNode.F = neighbourNode.G + neighbourNode.H

			// Form the path.
			pathLen := len(node.Path) + 1
			neighbourNode.Path = make(
				[]string, len(node.Path)+1)
			copy(neighbourNode.Path, node.Path)
			neighbourNode.Path[pathLen-1] = neighbourID

			queue.Push(neighbourNode)
			neighbourNode.Traversed = true
		}

		node.Traversed = true
	}

	fmt.Println(finish.Path)

	// Draw the tile map with the path.
	for nodeID, node := range graph {
		parts := strings.Split(nodeID, "_")
		i, err := strconv.Atoi(parts[0])
		handleError(err)
		j, err := strconv.Atoi(parts[1])
		handleError(err)

		if node.Traversed {
			tileMap[i][j] = "#"
		}

		if node == start {
			tileMap[i][j] = "S"
		}

		if node == finish {
			tileMap[i][j] = "F"
		}
	}

	for _, row := range tileMap {
		concat := strings.Join(row, ".")
		fmt.Println(concat)
	}
}

func handleError(err error) {
	if err != nil {
		panic(err)
	}
}