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

Question

I'm trying to do something a little bit different then the usual.

I have a 3D gridmap node setup and I'm trying to autogenerate the dots and connections using A* Instead of creating obstacles tiles, I'm creating walls in between the tiles, so the tiles are still walkable, you just cannot pass through a wall . I figure it that out all already

but I have no idea how to code how to connect the points in a easy way and not connect points that has walls in between...

I'm using a RaycastCast Node to detect the wall, and his position as it walk through every gridtile

but I can't figure it out a nested loop to find the neighbors points to connect

this is what I tried to do (obviously get_closest_point() is not working the way I wanted). If I could get a point using only Vector3 Coordinates, I think I could make it work.

EXTRA: if you guys can show me a way to clean the code, especially on the "FORs" syntaxes, because I kind don't know what I'm doing

Any other clean code recommendations would be amazing and very much welcomed

At the end has a visual draw(image) of the logic of the idea.

onready var rc = $RayCast
onready var aS = AStar.new()
var floor_size = Vector3(12,0,12)
var origin = Vector3(-5.5, 0.5, -2.5)
var FRONT = Vector3(1,0,0)
var RIGHT = Vector3(0,0,1)
var BACK = Vector3(-1,0,0)
var LEFT = Vector3(-1,0,0)


func set_walkable():
    var value = 0
    var value2 = 0
    var i = 0
    for _length in range (origin.x, origin.x + floor_size.x + 1):
        value += 1 
        value2 = 0
        for _width in range(origin.z, origin.z + floor_size.z):
            i += 1
            value2 += 1
            aS.add_point(i,  Vector3(origin.x + value -1 , 0.5, origin.z + value2 -1) , 1.0)
    value = 0       
    for _u in range(origin.x, origin.x + floor_size.x + 1):
        value += 1 
        value2 = 0
        for _v in range(origin.z, origin.z + floor_size.z):
            value2 += 1
            var from = aS.get_closest_point(Vector3(origin.x + value ,0.5,  origin.z + value2) ) # Current
            rc.translation = Vector3(origin.x + value -1 ,0.5,  origin.z + value2 -1)
            draw_points()
            print(rc.translation)
            rc.cast_to = FRONT
            var to = aS.get_closest_point(rc.translation) # Front
            if from != -1 and !rc.is_colliding():
                aS.connect_points(from, to)
                draw_connections(Vector3(rc.translation.x + 0.5,rc.translation.y,rc.translation.z))
            rc.cast_to = BACK
            to = aS.get_closest_point(rc.translation) # back
            if from != -1 and !rc.is_colliding():
                aS.connect_points(from, to)
                draw_connections(Vector3(rc.translation.x + -0.5,rc.translation.y,rc.translation.z))
            rc.cast_to = RIGHT
            to = aS.get_closest_point(rc.translation) # right
            if from != -1 and !rc.is_colliding():
                aS.connect_points(from, to)
                draw_connections(Vector3(rc.translation.x,rc.translation.y,rc.translation.z + 0.5))
            rc.cast_to = LEFT
            to = aS.get_closest_point(rc.translation) # left
            if from != -1 and !rc.is_colliding():
                aS.connect_points(from, to)
                draw_connections(Vector3(rc.translation.x + 0.5,rc.translation.y,rc.translation.z + -0.5))


func draw_points(): # Make points visible
    var cube = MeshInstance.new() 
    cube.mesh = CubeMesh.new()
    cube.translation = rc.translation
    cube.scale = Vector3(0.25,0.25,0.25)
    add_child(cube)
    print("Cubo adicionado")

func draw_connections(position): # Make connections visible
    var line = MeshInstance.new()
    line.mesh = PlaneMesh.new()
    line.scale = Vector3(0.03,0.03,0.03)
    line.translation = position
    add_child(line)
    print("Cubo adicionado")

Answer 1

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)