Godot A* pathfinding

pathfinding.gd

func grid_to_world(position):
    # (0,8) is the offset to the middle of the tile
    return map_to_world(position) + Vector2(0, 8)

func world_to_grid(position):
    return world_to_map(position)

func get_tile_cost(pos):
    # placeholder
    return 1.0

func get_tile_weighing_category(pos):
    # placeholder
    return 'normal'

func _heap_pop(heap, f_scores):
    # naive O(n) implementation. Only invariant of this function is
    # that it returns the smallest element in the data structure.
    # Can be optimized to an actual heap with O(log n) if it becomes necessary
    var minimum_i = 0
    var current_minimum = 1000000000
    if f_scores.has(heap[0]):
        current_minimum = f_scores[heap[0]]

    for i in range(1, heap.size()):
        var this_score = 1000000000
        if f_scores.has(heap[i]):
            this_score = f_scores[heap[i]]
        if this_score < current_minimum:
            minimum_i = i
            current_minimum = this_score

    var ret = heap[minimum_i]
    heap.remove(minimum_i)
    return ret

func _heap_push(heap, val, f_scores):
    heap.append(val)

func get_neighbors(field):
    var neighbors = []
    for move in [Vector2(-1, 0), Vector2(1, 0), Vector2(0, 1), Vector2(0, -1)]:
        var potential_neighbor = Vector2(int(field.x + move.x), int(field.y + move.y))
        var world_point = grid_to_world(potential_neighbor)
        if (world_point - get_node("WalkMap").get_closest_point(world_point)).length_squared() < 0.1:
            neighbors.append(potential_neighbor)
    return Vector2Array(neighbors)

func _heuristic_score(prev, from, to):
    # a bit of a convoluted heuristic score that punishes taking many turns
    return (to - from).length_squared() * (from - prev).angle_to(to - from)

func calculate_path(from, to, weights={}):
    # Perform an A* to calculate a good way to the target
    # Use WalkMap to check whether a tile is accessible
    # Weights can be used to prefer certain categories of field costs.
    # (I'll document this better later, triple promise!)
    #
    # closely following https://en.wikipedia.org/wiki/A*_search_algorithm#Pseudocode
    
    # naively arrays, could be replaced with heaps and search trees in case of performance issues
    var closed = []
    var open = [from]
    var predecessors = {}
    var g_scores = {from: 0}
    var turns = {from: 0}
    var f_scores = {to: (to - from).length_squared()}
    # shackle maximum checking size
    while open.size() > 0 and open.size() < 9999:
        var current = _heap_pop(open, f_scores)
        # if reached the goal, retrace steps
        if (current - to).length_squared() < 0.1:
            var path = []
            var f = to
            while (f - from).length_squared() > 0.1:
                path.insert(0, f)
                f = predecessors[f]
            return Vector2Array([from] + path)
        
        open.erase(current)
        closed.append(current)

        for neighbor in get_neighbors(current):
            # skip checked nodes
            if closed.find(neighbor) > -1:
                continue

            var cost_modifier = 1.0
            var weight_category = get_tile_weighing_category(neighbor)
            if weights.has(weight_category):
                cost_modifier = weights[weight_category]

            var this_turns = turns[current]
            var turn_cost = 0
            if predecessors.has(current):
                turn_cost = abs(sin((current - predecessors[current]).angle_to(neighbor - current)))

            if turn_cost > 0.5:
                this_turns += 1

            var move_cost = get_tile_cost(neighbor)
            var tentative_g_score = g_scores[current] + move_cost * cost_modifier
            tentative_g_score += this_turns * 1.5

            if open.find(neighbor) == -1:
                _heap_push(open, neighbor, f_scores)
            elif tentative_g_score >= g_scores[neighbor]:
                continue # found a worse path
            
            # found a better path
            predecessors[neighbor] = current
            turns[neighbor] = this_turns
            g_scores[neighbor] = tentative_g_score
            f_scores[neighbor] = tentative_g_score + (to - neighbor).length_squared() + turn_cost
        
    return null

Again Thanks for the explanation. Over the next few days I will do as you have advised and see what results I get.