python-pathfinding/pathfinding/core/util.py at python2 · brean/python-pathfinding · GitHub

History

132 lines (108 loc) · 3.19 KB

Raw

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

# -*- coding: utf-8 -*-

import copy

import math

# square root of 2 for diagonal distance

SQRT2 = math.sqrt(2)

def backtrace(node):

"""

Backtrace according to the parent records and return the path.

(including both start and end nodes)

"""

path = [(node.x, node.y)]

while node.parent:

node = node.parent

path.append((node.x, node.y))

path.reverse()

return path

def bi_backtrace(node_a, node_b):

"""

Backtrace from start and end node, returns the path for bi-directional A*

(including both start and end nodes)

"""

path_a = backtrace(node_a)

path_b = backtrace(node_b)

path_b.reverse()

return path_a + path_b

def raytrace(coords_a, coords_b):

line = []

x0, y0 = coords_a

x1, y1 = coords_b

dx = x1 - x0

dy = y1 - y0

t = 0

grid_pos = [x0, y0]

t_for_one = \

abs(1.0 / dx) if dx > 0 else 10000, \

abs(1.0 / dy) if dy > 0 else 10000

frac_start_pos = (x0 + .5) - x0, (y0 + .5) - y0

t_for_next_border = [

(1 - frac_start_pos[0] if dx < 0 else frac_start_pos[0]) * t_for_one[0],

(1 - frac_start_pos[1] if dx < 0 else frac_start_pos[1]) * t_for_one[1]

]

step = \

1 if dx >= 0 else -1, \

1 if dy >= 0 else -1

while t <= 1:

line.append(copy.copy(grid_pos))

index = 0 if t_for_next_border[0] <= t_for_next_border[1] else 1

t = t_for_next_border[index]

t_for_next_border[index] += t_for_one[index]

grid_pos[index] += step[index]

return line

def bresenham(coords_a, coords_b):

'''

Given the start and end coordinates, return all the coordinates lying

on the line formed by these coordinates, based on Bresenham's algorithm.

http://en.wikipedia.org/wiki/Bresenham's_line_algorithm#Simplification

'''

line = []

x0, y0 = coords_a

x1, y1 = coords_b

dx = abs(x1 - x0)

dy = abs(y1 - y0)

sx = 1 if x0 < x1 else -1

sy = 1 if y0 < y1 else -1

err = dx - dy

while True:

line += [[x0, y0]]

if x0 == x1 and y0 == y1:

break

e2 = err * 2

if e2 > -dy:

err = err - dy

x0 = x0 + sx

if e2 < dx:

err = err + dx

y0 = y0 + sy

return line

def expand_path(path):

'''

Given a compressed path, return a new path that has all the segments

in it interpolated.

'''

expanded = []

if len(path) < 2:

return expanded

for i in range(len(path) - 1):

expanded += bresenham(path[i], path[i + 1])

expanded += [path[:-1]]

return expanded

def smoothen_path(grid, path, use_raytrace=False):

x0, y0 = path[0]

sx, sy = path[0]

new_path = [[sx, sy]]

interpolate = raytrace if use_raytrace else bresenham

last_valid = path[1]

for coord in path[2:-1]:

line = interpolate([sx, sy], coord)

blocked = False

for test_coord in line[1:]:

if not grid.walkable(test_coord[0], test_coord[1]):

blocked = True

break

if not blocked:

new_path.append(last_valid)

sx, sy = last_valid

last_valid = coord

new_path.append(path[-1])

return new_path

python-pathfinding/pathfinding/core/util.py at python2 · brean/python-pathfinding · GitHub

Expand file tree

Breadcrumbs

util.py

Latest commit

History

Breadcrumbs

util.py

File metadata and controls