How I connect dots using A* (Astart) pathfinding system? in GODOT

Convert between Coordinates and Point ids

Let us establish a mapping between coordinates and point ids. Given that we have a floor_size, this is easy:

func vector_to_id(vector:Vector3, size:Vector3) -> int:
    return int(int3(vector).dot(dimension_size(size)))

func id_to_vector(id:int, size:Vector3) -> Vector3:
    var s:Vector3 = dimension_size(size)
    var z:int = int(id / s.z)
    var y:int = int((id % int(s.z)) / s.y)
    var x:int = id % int(s.y)
    return Vector3(x, y, z)

func int3(vector:Vector3) -> Vector3:
    return Vector3(int(vector.x), int(vector.y), int(vector.z))

func dimension_size(size:Vector3) -> Vector3:
    return Vector3(1, int(size.x + 1), int(size.x + 1) * int(size.y + 1))

Possible optimizations:

• Store dimension_size(floor_size) and use that directly.
• Skip calling int3 on the condition that the values you pass to vector_to_id are guaranteed to be integer.

We will need a function to get the total number of points:

func total_size(size:Vector3) -> int:
    return int(size.x + 1) * int(size.y + 1) * int(size.z + 1)

Explanation

Let us start at 1D (one dimension). We will only have one coordinate. So we have an hypothetical Vector1 that has an x property. We are simply putting things in a line.

Then the mapping is trivial: to convert from the coordinates to the id, we take id = int(vector.x), and if we want the coordinate we simply do vector = Vector1(id).

Now, let us move to 2D. We have Vector2 with x and y. Thankfully we have a size (there are ways to do the mapping when the size is not known, but having a size is convenient).

Thus, we will be doing a 2D grid, with some width and height. The y coordinate tells us the row in which we are, and x tells us the position in the row.

Then if we have some id, we need to figure out how many rows we need to get there, and then in what position in that row we are. Figuring out the row is easy, we divide by the width of the grid. And the position in the row is the reminder. One caveat: We are measuring from 0 (so a width of 0 actually means 1 element per row).

We have:

func id_to_vector(id:int, size:Vector2) -> Vector2:
    var y:int = int(id / (size.x + 1))
    var x:int = id % int(size.x + 1)
    return Vector2(x, y)

How about going the other way around? Well, we multiply y for the length of a row (the width), and add x:

func vector_to_id(vector:Vector2, size:Vector2) -> int:
    return int(vector.x) + int(vector.y) * int(size.x + 1)

Notice:

• We didn't need size.y.
• We need size.x + 1 in both functions.
• vector_to_id looks very similar to a dot product.

Thus, let us make a new function that returns the vector with which we would be making the dot product:

func dimension_size(size:Vector2) -> Vector2:
    return Vector2(1, int(size.x + 1))

And use it:

func vector_to_id(vector:Vector2, size:Vector2) -> int:
    return int(vector.dot(dimensional_size(size)))

func id_to_vector(id:int, size:Vector2) -> Vector2:
    var s = dimensional_size(size)
    var y:int = int(id / int(s.y))
    var x:int = id % int(s.y)
    return Vector2(x, y)

Note If there is no guarantee that vector only has integers in vector_to_id, the fractional part in the dot product make lead to a wrong result. Which is why I have a function to make it have only integer.

Time for 3D. We have Vector3 with x, y and z. We are making a 3D grid. Now the z will tell us the layer, and each layer is a 2D grid.

Let us review dimensional_size, We had:

func dimension_size(size:Vector2) -> Vector2:
    return Vector2(1, int(size.x + 1))

That is the size of an element (1), the size of a row(size.x + 1), we need to add the size of a layer. That is, the size of the 2D grid, which is just width times height.

func dimension_size(size:Vector3) -> Vector3:
    return Vector3(1, int(size.x + 1), int(size.x + 1) * int(size.y + 1))

And how do we get z from the id? We divide by the size of a grid (so we know on what grid we are). Then from the reminder of that division we can find y:

func vector_to_id(vector:Vector3, size:Vector3) -> int:
    return int(vector.dot(dimensional_size(size)))

func id_to_vector(id:int, size:Vector3) -> Vector3:
    var s = dimensional_size(size)
    var z:int = int(id / int(s.z))
    var y:int = int(int(id % int(s.z)) / int(s.y))
    var x:int = id % int(s.y)
    return Vector2(x, y, z)

In fact, technically, all these coordinates are computed on the same form:

func id_to_vector(id:int, size:Vector3) -> Vector3:
    var s = dimensional_size(size)
    var tot = total_size(size)
    var z:int = int(int(id % int(tot)) / int(s.z))
    var y:int = int(int(id % int(s.z)) / int(s.y))
    var x:int = int(int(id % int(s.y)) / int(s.x))
    return Vector2(x, y, z)

Except, there is no need to take the reminder with the total size because id should always be less than that. And there is no need to divide by s.x because the size of a single element is always 1. And I also removed some redundant int casts.

What is total_size? The next element of dimensional_size, of course:

func dimension_size(size:Vector3) -> Vector3:
    return Vector3(1, int(size.x + 1), int(size.x + 1) * int(size.y + 1))

func total_size(size:Vector3) -> int:
    return int(size.x + 1) * int(size.y + 1) * int(size.z + 1)

Checking connectivity

And a way to check connectivity:

func check_connected(start:Vector3, end:Vector3) -> bool:
    rc.transform.origin = start
    rc.cast_to = end
    rc.force_update_transform()
    rc.force_raycast_update()
    return !raycast.is_colliding()

And you had the right idea with FRONT, RIGHT, BACK and LEFT but put them in an array:

var offsets = [Vector3(1,0,0), Vector3(0,0,1), Vector3(-1,0,0), Vector3(-1,0,0)]

Note I'm calling force_update_transform and force_raycast_update because are doing multiple raycast checks on the same frame.

Populating AStar

Alright, enough setup, we can now iterate:

for id in total_size(floor_size):
    pass

On each iteration, we need to get the vector:

for id in total_size(floor_size):
    var vector = id_to_vector(id, floor_size)

Posible optimization: Iterate over the vector coordinates directly to avoid calling id_to_vector.

We can add the vector to AStar:

for id in total_size(floor_size):
    var vector = id_to_vector(id, floor_size)
    aS.add_point(id, vector)

Next we need the adjacent vectors:

for id in total_size(floor_size):
    var vector = id_to_vector(id, floor_size)
    aS.add_point(id, vector)
    for offset in offsets:
        var adjacent = vector + offset

Let us add them to AStar too:

for id in total_size(floor_size):
    var vector = id_to_vector(id, floor_size)
    aS.add_point(id, vector)
    for offset in offsets:
        var adjacent = vector + offset
        var adjacent_id = vector_to_id(adjacent, floor_size)
        aS.add_point(adjacent_id, adjacent)

Possible optimizations:

• Do not add if has_point returns true.
• If the id of the adjacent vector is lower, do not process it.
• Modify offsets so that you only check adjacent position that are yet to be added (and thus preventing the prior two cases).

Let us check connectivity:

for id in total_size(floor_size):
    var vector = id_to_vector(id, floor_size)
    aS.add_point(id, vector)
    for offset in offsets:
        var adjacent = vector + offset
        var adjacent_id = vector_to_id(adjacent, floor_size)
        aS.add_point(adjacent_id, adjacent)
        if check_connected(vector, adjacent):
            pass

And tell the AStar about the connectivity:

for id in total_size(floor_size):
    var vector = id_to_vector(id, floor_size)
    aS.add_point(id, vector)
    for offset in offsets:
        var adjacent = vector + offset
        var adjacent_id = vector_to_id(adjacent, floor_size)
        aS.add_point(adjacent_id, adjacent)
        if check_connected(vector, adjacent):
            connect_points(id, adjacent_id)