v1/ai/pathfinding

{
	return &Pathfinder{cancels: make(map[int]context.CancelFunc)}
}

{
	return y >= 0 && y < len(grid) && x >= 0 && x < len(grid[0])
}

{
	if len(grid) == 0 || len(grid[0]) == 0 {
		return nil, false, ErrOutOfBounds
	}
	sx, sy := int(start.X), int(start.Y)
	gx, gy := int(goal.X), int(goal.Y)
	if !inBounds(grid, sx, sy) || !inBounds(grid, gx, gy) {
		return nil, false, ErrOutOfBounds
	}
	if grid[sy][sx] == 1 || grid[gy][gx] == 1 {
		return nil, false, ErrBlocked
	}
	if sx == gx && sy == gy {
		return []m.Vec2{{float32(sx), float32(sy)}}, true, nil
	}

	// Manhattan heuristic (admissible in 4-dir grid with unit costs).
	h := func(x, y int) float32 { return float32(math.Abs(float64(x-gx)) + math.Abs(float64(y-gy))) }

	startNode := &gridNode{pos: m.Vec2{float32(sx), float32(sy)}, g: 0, f: h(sx, sy)}
	open := priorityQueue{startNode}
	heap.Init(&open)
	visited := map[[2]int]*gridNode{{sx, sy}: startNode}

	for open.Len() > 0 {
		select {
		case <-ctx.Done():
			return nil, false, context.Canceled
		default:
		}
		current := heap.Pop(&open).(*gridNode)
		x, y := int(current.pos.X), int(current.pos.Y)
		if x == gx && y == gy {
			return reconstructGrid(current), true, nil
		}
		// 4-neighbors
		for _, n := range [][2]int{{x + 1, y}, {x - 1, y}, {x, y + 1}, {x, y - 1}} {
			nx, ny := n[0], n[1]
			if !inBounds(grid, nx, ny) || grid[ny][nx] == 1 {
				continue
			}
			gScore := current.g + 1
			key := [2]int{nx, ny}
			if v, ok := visited[key]; ok && gScore >= v.g {
				continue
			}
			node := &gridNode{
				pos:    m.Vec2{float32(nx), float32(ny)},
				g:      gScore,
				f:      gScore + h(nx, ny),
				parent: current,
			}
			visited[key] = node
			heap.Push(&open, node)
		}
	}
	// Exhausted without reaching goal: not found.
	return nil, false, nil
}

{
	var path []m.Vec2
	for n != nil {
		path = append(path, n.pos)
		n = n.parent
	}
	// reverse in-place
	for i := 0; i < len(path)/2; i++ {
		path[i], path[len(path)-1-i] = path[len(path)-1-i], path[i]
	}
	return path
}

{
	if mesh.Polygons == nil {
		return nil, false, errors.New("empty navmesh")
	}
	if _, ok := mesh.Polygons[start]; !ok {
		return nil, false, errors.New("start polygon not found")
	}
	if _, ok := mesh.Polygons[goal]; !ok {
		return nil, false, errors.New("goal polygon not found")
	}
	if start == goal {
		return []int{start}, true, nil
	}

	// Euclidean heuristic on polygon centers.
	h := func(a int) float32 {
		sa := mesh.Polygons[a].Center
		sb := mesh.Polygons[goal].Center
		dx := sa.X - sb.X
		dy := sa.Y - sb.Y
		return float32(math.Hypot(float64(dx), float64(dy)))
	}

	startNode := &navNode{id: start, g: 0, f: h(start)}
	open := priorityQueueNav{startNode}
	heap.Init(&open)
	visited := map[int]*navNode{start: startNode}

	for open.Len() > 0 {
		select {
		case <-ctx.Done():
			return nil, false, context.Canceled
		default:
		}
		current := heap.Pop(&open).(*navNode)
		if current.id == goal {
			return reconstructNav(current), true, nil
		}
		for _, nb := range mesh.Polygons[current.id].Neighbors {
			// Defensive: skip neighbors not present in the mesh.
			if _, ok := mesh.Polygons[nb]; !ok {
				continue
			}
			gScore := current.g + 1 // uniform edge cost
			if v, ok := visited[nb]; ok && gScore >= v.g {
				continue
			}
			node := &navNode{id: nb, g: gScore, f: gScore + h(nb), parent: current}
			visited[nb] = node
			heap.Push(&open, node)
		}
	}
	// Exhausted without reaching goal: not found.
	return nil, false, nil
}

{
	var path []int
	for n != nil {
		path = append(path, n.id)
		n = n.parent
	}
	// reverse in-place
	for i := 0; i < len(path)/2; i++ {
		path[i], path[len(path)-1-i] = path[len(path)-1-i], path[i]
	}
	return path
}

{
	pf := New()
	grid := Grid{
		{0, 0, 0},
		{1, 1, 0},
		{0, 0, 0},
	}
	start := m.Vec2{0, 0}
	goal := m.Vec2{2, 2}
	_, ch := pf.RequestGridPath(context.Background(), grid, start, goal)
	res := <-ch
	if !res.Found {
		t.Fatalf("expected path, got err %v", res.Err)
	}
	if len(res.Path) == 0 || res.Path[0] != start || res.Path[len(res.Path)-1] != goal {
		t.Fatalf("unexpected path %v", res.Path)
	}
}

{
	mesh := NavMesh{Polygons: map[int]Poly{
		1: {ID: 1, Center: m.Vec2{0, 0}, Neighbors: []int{2}},
		2: {ID: 2, Center: m.Vec2{1, 0}, Neighbors: []int{1, 3}},
		3: {ID: 3, Center: m.Vec2{2, 0}, Neighbors: []int{2}},
	}}
	pf := New()
	_, ch := pf.RequestNavMeshPath(context.Background(), mesh, 1, 3)
	res := <-ch
	if !res.Found {
		t.Fatalf("expected path, got err %v", res.Err)
	}
	if len(res.Path) != 3 || res.Path[0] != 1 || res.Path[2] != 3 {
		t.Fatalf("unexpected path %v", res.Path)
	}
}

{
	pf := New()
	// Large grid to give time for cancellation.
	grid := make(Grid, 100)
	for i := range grid {
		grid[i] = make([]int, 100)
	}
	start := m.Vec2{0, 0}
	goal := m.Vec2{99, 99}
	id, ch := pf.RequestGridPath(context.Background(), grid, start, goal)
	pf.Cancel(id)
	res := <-ch
	if res.Err != context.Canceled {
		t.Fatalf("expected context.Canceled, got %v", res.Err)
	}
}