Esempio n. 1
0
def myFindPath(start, end, mesh, grid, tilesize=16):
    """ Uses astar to find a path from start to end,
        using the given mesh and tile grid.
        >>> grid = [[0,0,0,0,0],[0,0,0,0,0],[0,0,1,0,0],[0,0,0,0,0],[0,0,0,0,0]]
        >>> mesh = make_nav_mesh([(2,2,1,1)],(0,0,4,4),1)
        >>> find_path((0,0),(4,4),mesh,grid,1)
        [(4, 1), (4, 4)]
    """
    if not line_intersects_grid(start, end, grid, tilesize):
        return ([end], point_dist(start, end), True)
    direct = False
    mesh = copy.deepcopy(mesh)
    mesh[start] = dict([(n, point_dist(start,n)) for n in mesh if not line_intersects_grid(start,n,grid,tilesize)])
    if end not in mesh:
        endconns = [(n, point_dist(end,n)) for n in mesh if not line_intersects_grid(end,n,grid,tilesize)]
        for n, dst in endconns:
            mesh[n][end] = dst
    neighbours = lambda n: mesh[n].keys()
    cost       = lambda n1, n2: mesh[n1][n2]
    goal       = lambda n: n == end
    heuristic  = lambda n: ((n[0]-end[0]) ** 2 + (n[1]-end[1]) ** 2) ** 0.5
    nodes, length = astar(start, neighbours, goal, 0, cost, heuristic)
    if len(nodes) == 1:
        direct = True
    return (nodes, length, direct)
Esempio n. 2
0
def bangBangStar(start, startAngle, end, mesh, lineMesh, grid, tilesize, max_turn, max_speed):
    if not line_intersects_grid(start, end, grid, tilesize):
        return ([end], point_dist(start, end), True)
    direct = False
    lineMesh = copy.deepcopy(lineMesh)
    lineMesh[(start,start)] = {}
    for n in mesh:
        if not line_intersects_grid(start, n, grid, tilesize):
            if start != n:
                lineMesh[(start,start)][(start,n)] = max_speed * \
                    (math.fabs(angle_fix(getAngle(start,n) - startAngle))//max_turn) + \
                    point_dist(start,n)
            if start not in mesh:
                lineMesh[(start,n)] = {}
                for n2 in mesh[n]:
                    lineMesh[(start,n)][(n,n2)] = max_speed * \
                        (math.fabs(angle_fix(getAngle(n,n2)-getAngle(start,n)))//max_turn) + \
                        point_dist(n, n2)
    if end != start:
        if end not in mesh:
            for n2 in mesh.keys() + [start]:
                if not line_intersects_grid(end, n2, grid, tilesize):
                    lineMesh[(n2,end)] = dict([((end,end),0)])
                    for n in mesh.keys() + [start]:
                        if (n, n2) in lineMesh:
                            lineMesh[(n,n2)][(n2,end)] = max_speed * \
                        (math.fabs(angle_fix(getAngle(n2,end)-getAngle(n,n2)))//max_turn) + \
                        point_dist(n2, end)
        else:
            for n in mesh.keys() + [start]:
                if (n, end) in lineMesh:
                    lineMesh[(n,end)][(end,end)] = 0
    neighbours = lambda n: lineMesh[n].keys()
    cost       = lambda n1, n2: lineMesh[n1][n2]
    goal       = lambda n: n == (end,end)
    heuristic  = lambda n: point_dist(n[1], end)
    nodes, length = astar((start,start), neighbours, goal, 0, cost, heuristic)
    if len(nodes) == 1:
        direct = True
    path = []
    for (n,n2) in nodes:
            path += [n2]
    return (path, length, direct)