Pathfinding using A8-like algo, but attempts to minimize damage within a travel budget

pathfinding.py

class Path:
    _offset_to_distance: dict[tuple[int, int], float] = {
        (0, 0): 0,
        (0, 1): 1,
        (0, -1): 1,
        (1, 0): 1,
        (1, 1): 2 ** 0.5,
        (1, -1): 2 ** 0.5,
        (-1, 0): 1,
        (-1, 1): 2 ** 0.5,
        (-1, -1): 2 ** 0.5,
    }

    @classmethod
    def init_with_damage_and_travel_max(
        cls,
        difficulty_cost: np.ndarray,
        damage_cost: np.ndarray,
        start_r: int,
        start_c: int,
        end_r: int,
        end_c: int,
        travel_max: int,
    ) -> Optional['Path']:
        frontier: queue.PriorityQueue = queue.PriorityQueue()
        frontier.put((0, 0, 0, [(start_r, start_c)]))

        best_dict: dict[tuple[int, int], float] = {(start_r, start_c): 0.0}

        while not frontier.empty():
            damage_cur, estimate_cur, travel_cur, rc_list_cur = frontier.get()
            r, c = rc_list_cur[-1]

            if (r, c) == (end_r, end_c):
                return cls(rc_list_cur)

            for offset_r in (-1, 0, 1):
                for offset_c in (-1, 0, 1):
                    # Can't stay still
                    if offset_r == 0 and offset_c == 0:
                        continue

                    next_r = r + offset_r
                    next_c = c + offset_c

                    # Can't walk through walls
                    if difficulty_cost[next_r, next_c] >= 999:
                        continue

                    # Don't bother with places that are too far away from the goal
                    travel_next = (
                        travel_cur + difficulty_cost[next_r, next_c] + cls._offset_to_distance[(offset_r, offset_c)]
                    )
                    estimate_next = travel_next + math.sqrt((next_r - end_r) ** 2 + (next_c - end_c) ** 2)
                    if estimate_next > travel_max:
                        continue

                    # Can't revisit the same location, unless the travel is shorter.
                    # This could be finding a different route, or shorter path due to taking more damage.
                    damage_next = damage_cur + damage_cost[next_r, next_c]
                    best_key = (next_r, next_c)
                    if best_dict.get(best_key, 999) > travel_next:
                        best_dict[best_key] = travel_next
                    else:
                        continue

                    # If we get here, maybe the next step is worth checking out
                    frontier.put((damage_next, estimate_next, travel_next, rc_list_cur + [(next_r, next_c)]))

        return None

    def __init__(self, rc_list: list[tuple[int, int]]):
        self.rc_list: list[tuple[int, int]] = rc_list
        self.offset_list: list[tuple[int, int]] = [(0, 0)]

        for i in range(1, len(self.rc_list)):
            offset_tup = (self.rc_list[i][0] - self.rc_list[i - 1][0], self.rc_list[i][1] - self.rc_list[i - 1][1])
            self.offset_list.append(offset_tup)

            
# Testing
def _make_path(travel_max):
    difficulty_cost: np.ndarray = np.zeros((9, 9), dtype=np.float32)
    damage_cost: np.ndarray = np.zeros((9, 9), dtype=np.float32)

    difficulty_cost[0, :] = 999
    difficulty_cost[-1, :] = 999
    difficulty_cost[:, 0] = 999
    difficulty_cost[:, -1] = 999

    damage_cost[2, 0:7] = 1
    damage_cost[6, 0:7] = 1
    damage_cost[4, 2:9] = 1

    return Path.init_with_damage_and_travel_max(
        difficulty_cost,
        damage_cost,
        1,
        4,
        7,
        4,
        travel_max,
    )


def test_path():
    path = _make_path(5)
    assert path == None

    for i in [6, 7, 8]:
        path = _make_path(i)
        assert path.rc_list == [(1, 4), (2, 4), (3, 4), (4, 4), (5, 4), (6, 4), (7, 4)]

    for i in [9, 10]:
        path = _make_path(i)
        assert path.rc_list == [(1, 4), (2, 3), (3, 2), (4, 1), (5, 2), (6, 3), (7, 4)]

    for i in [11, 12, 13]:
        path = _make_path(i)
        assert path.rc_list == [(1, 4), (1, 5), (1, 6), (2, 7), (3, 7), (4, 7), (5, 7), (6, 7), (7, 6), (7, 5), (7, 4)]

    for i in [21]:
        path = _make_path(i)
        assert path.rc_list == [
            (1, 4),
            (1, 5),
            (1, 6),
            (2, 7),
            (3, 6),
            (3, 5),
            (3, 4),
            (3, 3),
            (3, 2),
            (4, 1),
            (5, 2),
            (5, 3),
            (5, 4),
            (5, 5),
            (5, 6),
            (6, 7),
            (7, 6),
            (7, 5),
            (7, 4),
        ]