Add pathfinding garbagio

cmd/prog/main.go

@@ -27,6 +27,8 @@ type Game struct {
 	cellX, cellY int
 	fromX, fromY int
 	toX, toY     int
+	path         []sauce.Node
+	pathSuccess  bool
 	whichFromTo  bool
 }
 
@@ -35,6 +37,10 @@ func NewGame() *Game {
 	g.world = sauce.NewWorld(3, 3)
 	g.world.HueristicLine(10, 0, 10, 10, 1)
 	g.world.HueristicLine(10, 13, 10, 30, 1)
+	g.fromX = -1
+	g.fromY = -1
+	g.toX = -1
+	g.toY = -1
 	return g
 }
 
@@ -62,10 +68,13 @@ func (g *Game) Update() error {
 	g.cellX = cursorX / CellSize
 	g.cellY = cursorY / CellSize
 	if cell := g.world.At(g.cellX, g.cellY); cell != nil {
+		var reroll bool
 		if inpututil.IsMouseButtonJustPressed(ebiten.MouseButtonLeft) {
 			cell.Hueristic++
+			reroll = true
 		} else if inpututil.IsMouseButtonJustPressed(ebiten.MouseButtonRight) {
 			cell.Hueristic--
+			reroll = true
 		} else if inpututil.IsMouseButtonJustPressed(ebiten.MouseButtonMiddle) {
 			if !g.whichFromTo {
 				g.fromX, g.fromY = g.cellX, g.cellY
@@ -73,6 +82,13 @@ func (g *Game) Update() error {
 				g.toX, g.toY = g.cellX, g.cellY
 			}
 			g.whichFromTo = !g.whichFromTo
+			reroll = true
+		}
+		if reroll {
+			if g.fromX != -1 && g.fromY != -1 && g.toX != -1 && g.toY != -1 {
+				g.path, g.pathSuccess = sauce.FindPath(g.world, g.fromX, g.fromY, g.toX, g.toY)
+			}
+
 		}
 	}
 	return nil
@@ -117,6 +133,18 @@ func (g *Game) Draw(screen *ebiten.Image) {
 			}
 		}
 	}
+	// Draw path dots overtop.
+	for _, node := range g.path {
+		x := node.X*CellSize + CellSize/2
+		y := node.Y*CellSize + CellSize/2
+
+		c := color.RGBA{0, 255, 0, 255}
+		if !g.pathSuccess {
+			c = color.RGBA{255, 0, 0, 255}
+		}
+
+		vector.DrawFilledCircle(screen, float32(x), float32(y), 5, c, false)
+	}
 	ebitenutil.DebugPrint(screen, fmt.Sprintf("TPS: %0.2f\nFPS: %0.2f", ebiten.ActualTPS(), ebiten.ActualFPS()))
 }
 

sauce/astar.go

@@ -1,85 +1,100 @@
 package sauce
 
-type Node struct {
-	parent  *Node
-	x, y    int
-	f, g, h int
+import (
+	"math"
+)
+
+type Coord struct {
+	X, Y int16
 }
 
-func FindPath(w *World, fromX, fromY int, toX, toY int) []Node {
-	openList := make([]*Node, 0)
-	closedList := make([]*Node, 0)
+type Node struct {
+	Coord
+	parent *Node
+	g      int
+	f      int
+	d      int
+}
 
-	startNode := &Node{x: fromX, y: fromY}
-	endNode := &Node{x: toX, y: toY}
+type Nodes map[Coord]*Node
 
-	openList = append(openList, startNode)
+func FindPath(w *World, fromX, fromY int, toX, toY int) ([]Node, bool) {
+	var open []*Node
+	visited := make(Nodes)
 
-	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
+	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
 			}
 		}
-		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 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
+			return path, true
 		}
+		// Remove current from the open set.
+		open = append(open[:currentI], open[currentI+1:]...)
 
-		var neighbors []Node
+		// Get neighbors.
+		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 {
+				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
 				}
-				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})
-				}
+				neighbors = append(neighbors, &Node{Coord: coord, parent: current, g: math.MaxUint16, f: math.MaxUint16})
 			}
 		}
 
+		// Iterate over neighbors.
 		for _, neighbor := range neighbors {
-			exists := false
-			for _, node := range closedList {
-				if node.x == neighbor.x && node.y == neighbor.y {
-					exists = true
-					break
+			tentativeG := current.g + 1
+			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)
 			}
-			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
+
+	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
 }