Godot AStar Dev 1

gistfile1.txt

extends Spatial


var gridCols = 10
var gridRows = 10

var tileWidth = 1
var tileHeight = 1

var pathType = 'cardnial'
var grid = {}
var astar = AStar.new()

var obstacles = []

onready var cam = $CamPivot

var startVector = Vector3(0,0,0)
var endVector = Vector3(8,0,8)



func _ready():
    #default setup
    makeGrid()
    drawGrid()
    connectCardinal()
    refresh()
    
func refresh():
    set_cam()
    makePath()
    
func set_cam():
    var move_vec = Vector3(gridRows/2, 10, gridCols/2)
    cam.set_translation(move_vec)
    var plane = MeshInstance.new()
    plane.mesh = PlaneMesh.new()
    plane.translate(Vector3(gridRows/2, -0.1, gridCols/2))
    plane.scale_object_local(Vector3(gridRows/2, 1, gridCols/2))    #.scaled(Vector3(gridRows/2, 1, gridCols/2))
    plane.create_convex_collision()
    add_child(plane)
    var material = SpatialMaterial.new()
    material.albedo_color = Color(1.0, 0.0, 0.0, 1.0)
    plane.set_surface_material(0, material) #   int surface, Material material )
    
func clicked(pos, flag):
    print("CLICKED: ", pos, ", ", flag)
    var x = int(pos.x)
    var y = int(pos.z)
    #   AMMEND AND TEST BOUNDS
    if flag == 1:
        set_start(x, y)
    elif flag == 2:
        set_end(x, y)
    elif flag == 3:
        set_obstacle(x, y)
    refresh()
    
func set_start(x, y):
    startVector = Vector3(x,0,y)
    
func set_end(x, y):
    endVector = Vector3(x,0,y)
    
func set_obstacle(x, y):
    addObstacle(x, y)
    
func makeGrid():
    grid = {}
    var r = 0
    var c = 0
    var id = 0
    while r < gridRows:
        c = 0
        while c < gridCols:
            #grid[id] = {
            #   "neighbors":{
            #       "u":Vector3(c,0,r-1),
            #       "ru":Vector3(c+1,0,r-1),
            #       "r":Vector3(c+1,0,r),
            #       "rd":Vector3(c+1,0,r+1),
            #       "d":Vector3(c,0,r+1),
            #       "ld":Vector3(c-1,0,r+1),
            #       "l":Vector3(c-1,0,r),
            #       "lu":Vector3(c-1,0,r-1)
            #   },
            #   #gridpoint is the top right corner
            #   "gridPoint":Vector3(c,0,r),
            #   "center":Vector3(float(c)+float(tileWidth)/2,0,float(r)+float(tileHeight)/2)
            #}
            #astar.add_point(id,Vector3(c,0,r))
            add_point(id, c, r)
            c += 1
            id += 1
        r += 1
        
func add_point(id, x, y):
    grid[id] = {
        "neighbors":{
            "u":Vector3(x,0,y-1),
            "ru":Vector3(x+1,0,y-1),
            "r":Vector3(x+1,0,y),
            "rd":Vector3(x+1,0,y+1),
            "d":Vector3(x,0,y+1),
            "ld":Vector3(x-1,0,y+1),
            "l":Vector3(x-1,0,y),
            "lu":Vector3(x-1,0,y-1)
        },
        #gridpoint is the top right corner
        "gridPoint":Vector3(x,0,y),
        "center":Vector3(float(x)+float(tileWidth)/2,0,float(y)+float(tileHeight)/2)
    }
    astar.add_point(id,Vector3(x,0,y))
    
func drawGrid():
    var d = get_node("Grid")
    d.clear()
    for tile in grid:
        d.begin(Mesh.PRIMITIVE_LINE_STRIP, null)
        d.add_vertex(Vector3(grid[tile].gridPoint.x*tileWidth,0,grid[tile].gridPoint.z*tileHeight))
        d.add_vertex(Vector3(grid[tile].gridPoint.x*tileWidth+tileWidth,0,grid[tile].gridPoint.z*tileHeight))
        d.add_vertex(Vector3(grid[tile].gridPoint.x*tileWidth+tileWidth,0,grid[tile].gridPoint.z*tileHeight+tileHeight))
        d.add_vertex(Vector3(grid[tile].gridPoint.x*tileWidth,0,grid[tile].gridPoint.z*tileHeight+tileHeight))
        d.add_vertex(Vector3(grid[tile].gridPoint.x*tileWidth,0,grid[tile].gridPoint.z*tileHeight))
        d.end()
        
func drawObstacles():
    var d = get_node("Obstacles")
    d.clear()
    for tile in obstacles:
        d.begin(Mesh.PRIMITIVE_LINE_STRIP, null)
        d.add_vertex(Vector3(grid[tile].gridPoint.x*tileWidth,0,grid[tile].gridPoint.z*tileHeight))
        d.add_vertex(Vector3(grid[tile].gridPoint.x*tileWidth+tileWidth,0,grid[tile].gridPoint.z*tileHeight))
        d.add_vertex(Vector3(grid[tile].gridPoint.x*tileWidth+tileWidth,0,grid[tile].gridPoint.z*tileHeight+tileHeight))
        d.add_vertex(Vector3(grid[tile].gridPoint.x*tileWidth,0,grid[tile].gridPoint.z*tileHeight+tileHeight))
        d.add_vertex(Vector3(grid[tile].gridPoint.x*tileWidth,0,grid[tile].gridPoint.z*tileHeight))
        d.end()
        
func connectCardinal():
    for tile in grid:
        var ids = getDirectionIds(tile,["u","r","d","l"])
        for i in ids:
            if not astar.are_points_connected(tile,i):
                astar.connect_points(tile,i)
                
##directions are clockwise starting up 0 - 7
func getDirectionIds(id,dirs):
    var ids = []
    for d in dirs:
        for i in grid:
            if grid[i].gridPoint == grid[id].neighbors[d]:
                ids.push_back(i)
                continue
    return ids
    
func makePath():
    var startId = 0
    var endId = 0
    for i in grid:
        if grid[i].gridPoint == startVector:
            startId = i
        if grid[i].gridPoint == endVector:
            endId = i
    var path = astar.get_id_path(startId,endId)
    drawPath(path)
    
func drawPath(path):
    var d = get_node("DrawPath")
    d.clear()
    d.begin(Mesh.PRIMITIVE_LINE_STRIP, null)
    for p in path:
        d.add_vertex(Vector3(grid[p].center.x*tileWidth,0,grid[p].center.z*tileHeight))
    d.end()
    
func disconectPoint(tile):
    var ids = getDirectionIds(tile,["u","ru","r","rd","d","ld","l","lu"])
    for i in ids:
        if astar.are_points_connected(tile,i):
            astar.disconnect_points(tile,i)
            
func addObstacle(x, y):
    var v = Vector3(x,0,y)
    var tile
    for t in grid:
        if grid[t].gridPoint == v:
            tile = t
            continue
    print(tile)
    if tile in obstacles:
        dump_grid()
        add_point(tile, x, y)
        obstacles.erase(tile)
        #grid.sort()
        connectCardinal()
        dump_grid()
    else:
        disconectPoint(tile)
        obstacles.push_back(tile)
    drawObstacles()
    
func dump_grid():
    var keys = grid.keys();
    for key in keys: 
        var u = grid[key]
        print(key, ", ", u.gridPoint.x, ", ", u.gridPoint.z)