297 lines
6.9 KiB
Python
297 lines
6.9 KiB
Python
|
import importlib
|
||
|
|
import functools
|
||
|
|
import time
|
||
|
|
from math import hypot
|
||
|
|
|
||
|
|
from astar import AStar
|
||
|
|
|
||
|
|
import blocks
|
||
|
|
importlib.reload(blocks)
|
||
|
|
import utils
|
||
|
|
importlib.reload(utils)
|
||
|
|
|
||
|
|
class AStarTimeout(Exception):
|
||
|
|
pass
|
||
|
|
|
||
|
|
BLOCK_ABOVE = (0, +1, 0)
|
||
|
|
BLOCK_ABOVE2 = (0, +2, 0)
|
||
|
|
BLOCK_ABOVE3 = (0, +3, 0)
|
||
|
|
BLOCK_ABOVE4 = (0, +4, 0)
|
||
|
|
BLOCK_BELOW = (0, -1, 0)
|
||
|
|
BLOCK_BELOW2 = (0, -2, 0)
|
||
|
|
|
||
|
|
TRAVERSE_NORTH = (0, 0, -1)
|
||
|
|
TRAVERSE_SOUTH = (0, 0, +1)
|
||
|
|
TRAVERSE_EAST = (+1, 0, 0)
|
||
|
|
TRAVERSE_WEST = (-1, 0, 0)
|
||
|
|
ASCEND_NORTH = (0, +1, -1)
|
||
|
|
ASCEND_SOUTH = (0, +1, +1)
|
||
|
|
ASCEND_EAST = (+1, +1, 0)
|
||
|
|
ASCEND_WEST = (-1, +1, 0)
|
||
|
|
DESCEND_EAST = (+1, -1, 0)
|
||
|
|
DESCEND_WEST = (-1, -1, 0)
|
||
|
|
DESCEND_NORTH = (0, -1, -1)
|
||
|
|
DESCEND_SOUTH = (0, -1, +1)
|
||
|
|
DESCEND2_EAST = (+1, -2, 0)
|
||
|
|
DESCEND2_WEST = (-1, -2, 0)
|
||
|
|
DESCEND2_NORTH = (0, -2, -1)
|
||
|
|
DESCEND2_SOUTH = (0, -2, +1)
|
||
|
|
DESCEND3_EAST = (+1, -3, 0)
|
||
|
|
DESCEND3_WEST = (-1, -3, 0)
|
||
|
|
DESCEND3_NORTH = (0, -3, -1)
|
||
|
|
DESCEND3_SOUTH = (0, -3, +1)
|
||
|
|
DIAGONAL_NORTHEAST = (+1, 0, -1)
|
||
|
|
DIAGONAL_NORTHWEST = (-1, 0, -1)
|
||
|
|
DIAGONAL_SOUTHEAST = (+1, 0, +1)
|
||
|
|
DIAGONAL_SOUTHWEST = (-1, 0, +1)
|
||
|
|
PARKOUR_NORTH = (0, 0, -2)
|
||
|
|
PARKOUR_SOUTH = (0, 0, +2)
|
||
|
|
PARKOUR_EAST = (+2, 0, 0)
|
||
|
|
PARKOUR_WEST = (-2, 0, 0)
|
||
|
|
|
||
|
|
TRAVERSE = [
|
||
|
|
TRAVERSE_NORTH,
|
||
|
|
TRAVERSE_SOUTH,
|
||
|
|
TRAVERSE_EAST,
|
||
|
|
TRAVERSE_WEST,
|
||
|
|
]
|
||
|
|
|
||
|
|
ASCEND = [
|
||
|
|
ASCEND_NORTH,
|
||
|
|
ASCEND_SOUTH,
|
||
|
|
ASCEND_EAST,
|
||
|
|
ASCEND_WEST,
|
||
|
|
]
|
||
|
|
|
||
|
|
DESCEND = [
|
||
|
|
DESCEND_EAST,
|
||
|
|
DESCEND_WEST,
|
||
|
|
DESCEND_NORTH,
|
||
|
|
DESCEND_SOUTH,
|
||
|
|
]
|
||
|
|
|
||
|
|
DESCEND2 = [
|
||
|
|
DESCEND2_EAST,
|
||
|
|
DESCEND2_WEST,
|
||
|
|
DESCEND2_NORTH,
|
||
|
|
DESCEND2_SOUTH,
|
||
|
|
]
|
||
|
|
|
||
|
|
DESCEND3 = [
|
||
|
|
DESCEND3_EAST,
|
||
|
|
DESCEND3_WEST,
|
||
|
|
DESCEND3_NORTH,
|
||
|
|
DESCEND3_SOUTH,
|
||
|
|
]
|
||
|
|
|
||
|
|
DIAGONAL = [
|
||
|
|
DIAGONAL_NORTHEAST,
|
||
|
|
DIAGONAL_NORTHWEST,
|
||
|
|
DIAGONAL_SOUTHEAST,
|
||
|
|
DIAGONAL_SOUTHWEST,
|
||
|
|
]
|
||
|
|
|
||
|
|
PARKOUR = [
|
||
|
|
PARKOUR_NORTH,
|
||
|
|
PARKOUR_SOUTH,
|
||
|
|
PARKOUR_EAST,
|
||
|
|
PARKOUR_WEST,
|
||
|
|
]
|
||
|
|
|
||
|
|
HALF_PARKOUR = {
|
||
|
|
(0, 0, -2): (0, 0, -1),
|
||
|
|
(0, 0, 2): (0, 0, 1),
|
||
|
|
(2, 0, 0): (1, 0, 0),
|
||
|
|
(-2, 0, 0): (-1, 0, 0),
|
||
|
|
}
|
||
|
|
|
||
|
|
HYPOT_LUT = {
|
||
|
|
(0, -1): 1.0,
|
||
|
|
(0, 1): 1.0,
|
||
|
|
(1, 0): 1.0,
|
||
|
|
(-1, 0): 1.0,
|
||
|
|
(1, -1): 1.414,
|
||
|
|
(-1, -1): 1.414,
|
||
|
|
(1, 1): 1.414,
|
||
|
|
(-1, 1): 1.414,
|
||
|
|
(0, 2): 2.0,
|
||
|
|
(-2, 0): 2.0,
|
||
|
|
(2, 0): 2.0,
|
||
|
|
(0, -2): 2.0,
|
||
|
|
}
|
||
|
|
|
||
|
|
# larger started being slower
|
||
|
|
BLOCK_CACHE_SIZE = 2**14
|
||
|
|
|
||
|
|
class Pathfinder(AStar):
|
||
|
|
def __init__(self, chunks):
|
||
|
|
self.chunks = chunks
|
||
|
|
self.start_time = time.time()
|
||
|
|
|
||
|
|
@functools.lru_cache(maxsize=BLOCK_CACHE_SIZE)
|
||
|
|
def bair(self, p):
|
||
|
|
return self.chunks.get_block_at(*p) in blocks.NON_SOLID_IDS
|
||
|
|
|
||
|
|
@functools.lru_cache(maxsize=BLOCK_CACHE_SIZE)
|
||
|
|
def bavoid(self, p):
|
||
|
|
return self.chunks.get_block_at(*p) in blocks.AVOID_IDS
|
||
|
|
|
||
|
|
def check_traverse(self, node, offset):
|
||
|
|
dest = utils.padd(node, offset)
|
||
|
|
|
||
|
|
if not self.bair(dest):
|
||
|
|
return False
|
||
|
|
|
||
|
|
if self.bair(utils.padd(dest, BLOCK_BELOW)):
|
||
|
|
return False
|
||
|
|
|
||
|
|
if not self.bair(utils.padd(dest, BLOCK_ABOVE)):
|
||
|
|
return False
|
||
|
|
|
||
|
|
if self.bavoid(dest):
|
||
|
|
return False
|
||
|
|
|
||
|
|
if self.bavoid(utils.padd(dest, BLOCK_BELOW)):
|
||
|
|
return False
|
||
|
|
|
||
|
|
if self.bavoid(utils.padd(dest, BLOCK_ABOVE)):
|
||
|
|
return False
|
||
|
|
|
||
|
|
return True
|
||
|
|
|
||
|
|
def check_diagonal(self, node, offset):
|
||
|
|
if not self.check_traverse(node, offset):
|
||
|
|
return False
|
||
|
|
|
||
|
|
dest = utils.padd(node, offset)
|
||
|
|
thru1 = (node[0], node[1], dest[2])
|
||
|
|
thru2 = (dest[0], node[1], node[2])
|
||
|
|
|
||
|
|
if not self.bair(thru1):
|
||
|
|
return False
|
||
|
|
|
||
|
|
if not self.bair(utils.padd(thru1, BLOCK_ABOVE)):
|
||
|
|
return False
|
||
|
|
|
||
|
|
if self.bavoid(utils.padd(thru1, BLOCK_BELOW)):
|
||
|
|
return False
|
||
|
|
|
||
|
|
if not self.bair(thru2):
|
||
|
|
return False
|
||
|
|
|
||
|
|
if not self.bair(utils.padd(thru2, BLOCK_ABOVE)):
|
||
|
|
return False
|
||
|
|
|
||
|
|
if self.bavoid(utils.padd(thru2, BLOCK_BELOW)):
|
||
|
|
return False
|
||
|
|
|
||
|
|
return True
|
||
|
|
|
||
|
|
def check_ascend(self, node, offset):
|
||
|
|
if not self.check_traverse(node, offset):
|
||
|
|
return False
|
||
|
|
|
||
|
|
dest = utils.padd(node, offset)
|
||
|
|
|
||
|
|
if not self.bair(utils.padd(node, BLOCK_ABOVE2)):
|
||
|
|
return False
|
||
|
|
|
||
|
|
return True
|
||
|
|
|
||
|
|
def check_descend(self, node, offset):
|
||
|
|
if not self.check_traverse(node, offset):
|
||
|
|
return False
|
||
|
|
|
||
|
|
dest = utils.padd(node, offset)
|
||
|
|
|
||
|
|
if not self.bair(utils.padd(dest, BLOCK_ABOVE2)):
|
||
|
|
return False
|
||
|
|
|
||
|
|
return True
|
||
|
|
|
||
|
|
def check_descend2(self, node, offset):
|
||
|
|
if not self.check_descend(node, offset):
|
||
|
|
return False
|
||
|
|
|
||
|
|
dest = utils.padd(node, offset)
|
||
|
|
|
||
|
|
if not self.bair(utils.padd(dest, BLOCK_ABOVE3)):
|
||
|
|
return False
|
||
|
|
|
||
|
|
return True
|
||
|
|
|
||
|
|
def check_descend3(self, node, offset):
|
||
|
|
if not self.check_descend2(node, offset):
|
||
|
|
return False
|
||
|
|
|
||
|
|
dest = utils.padd(node, offset)
|
||
|
|
|
||
|
|
if not self.bair(utils.padd(dest, BLOCK_ABOVE4)):
|
||
|
|
return False
|
||
|
|
|
||
|
|
return True
|
||
|
|
|
||
|
|
def check_parkour(self, node, offset):
|
||
|
|
dest = utils.padd(node, offset)
|
||
|
|
half_offset = HALF_PARKOUR[offset]
|
||
|
|
middle = utils.padd(node, half_offset)
|
||
|
|
|
||
|
|
# dont jump if we can walk instead
|
||
|
|
if not self.bair(utils.padd(middle, BLOCK_BELOW)):
|
||
|
|
return False
|
||
|
|
|
||
|
|
if not self.check_ascend(node, offset):
|
||
|
|
return False
|
||
|
|
|
||
|
|
if not self.bair(utils.padd(dest, BLOCK_ABOVE2)):
|
||
|
|
return False
|
||
|
|
|
||
|
|
if not self.bair(utils.padd(middle, BLOCK_ABOVE)):
|
||
|
|
return False
|
||
|
|
|
||
|
|
if not self.bair(utils.padd(middle, BLOCK_ABOVE2)):
|
||
|
|
return False
|
||
|
|
|
||
|
|
return True
|
||
|
|
|
||
|
|
def neighbors(self, node):
|
||
|
|
results = []
|
||
|
|
|
||
|
|
for offset in TRAVERSE:
|
||
|
|
if self.check_traverse(node, offset):
|
||
|
|
results.append(utils.padd(node, offset))
|
||
|
|
for offset in DIAGONAL:
|
||
|
|
if self.check_diagonal(node, offset):
|
||
|
|
results.append(utils.padd(node, offset))
|
||
|
|
for offset in ASCEND:
|
||
|
|
if self.check_ascend(node, offset):
|
||
|
|
results.append(utils.padd(node, offset))
|
||
|
|
for offset in DESCEND:
|
||
|
|
if self.check_descend(node, offset):
|
||
|
|
results.append(utils.padd(node, offset))
|
||
|
|
for offset in DESCEND2:
|
||
|
|
if self.check_descend2(node, offset):
|
||
|
|
results.append(utils.padd(node, offset))
|
||
|
|
for offset in DESCEND3:
|
||
|
|
if self.check_descend3(node, offset):
|
||
|
|
results.append(utils.padd(node, offset))
|
||
|
|
for offset in PARKOUR:
|
||
|
|
if self.check_parkour(node, offset):
|
||
|
|
results.append(utils.padd(node, offset))
|
||
|
|
|
||
|
|
if not results:
|
||
|
|
if time.time() - self.start_time > 2.0:
|
||
|
|
raise(AStarTimeout)
|
||
|
|
|
||
|
|
return results
|
||
|
|
|
||
|
|
def distance_between(self, n1, n2):
|
||
|
|
(x1, y1, z1) = n1
|
||
|
|
(x2, y2, z2) = n2
|
||
|
|
return HYPOT_LUT[x2 - x1, z2 - z1]
|
||
|
|
|
||
|
|
def heuristic_cost_estimate(self, n1, n2):
|
||
|
|
(x1, y1, z1) = n1
|
||
|
|
(x2, y2, z2) = n2
|
||
|
|
return hypot(x2 - x1, z2 - z1)
|