From 100b4da80defd63d8010363d82e057543b4c7a54 Mon Sep 17 00:00:00 2001 From: Tanner Collin Date: Tue, 8 Sep 2020 15:33:42 -0600 Subject: [PATCH] Port over physics and pathfinding --- blocks.py | 216 +++++++++++++++++++++++++++++++++ bot.py | 166 ++++++++++++++++++++++--- main.py | 3 + packet_handlers.py | 35 +++++- path.py | 296 +++++++++++++++++++++++++++++++++++++++++++++ utils.py | 27 +++++ 6 files changed, 724 insertions(+), 19 deletions(-) create mode 100644 blocks.py create mode 100644 path.py create mode 100644 utils.py diff --git a/blocks.py b/blocks.py new file mode 100644 index 0000000..08e2e90 --- /dev/null +++ b/blocks.py @@ -0,0 +1,216 @@ +import json + +with open('mcdata/blocks.json') as f: + BLOCKS = json.load(f) + +AVOID = [ + 'minecraft:lava', + 'minecraft:water', + 'minecraft:fire', + 'minecraft:magma_block', + 'minecraft:oak_fence', + 'minecraft:oak_fence_gate', + 'minecraft:nether_brick_fence', + 'minecraft:spruce_fence_gate', + 'minecraft:birch_fence_gate', + 'minecraft:jungle_fence_gate', + 'minecraft:acacia_fence_gate', + 'minecraft:dark_oak_fence_gate', + 'minecraft:spruce_fence', + 'minecraft:birch_fence', + 'minecraft:jungle_fence', + 'minecraft:acacia_fence', + 'minecraft:dark_oak_fence', + 'minecraft:sweet_berry_bush', + 'minecraft:nether_portal', + 'minecraft:end_portal', + 'minecraft:cobblestone_wall', + 'minecraft:mossy_cobblestone_wall', + 'minecraft:brick_wall', + 'minecraft:prismarine_wall', + 'minecraft:red_sandstone_wall', + 'minecraft:mossy_stone_brick_wall', + 'minecraft:granite_wall', + 'minecraft:stone_brick_wall', + 'minecraft:nether_brick_wall', + 'minecraft:andesite_wall', + 'minecraft:red_nether_brick_wall', + 'minecraft:sandstone_wall', + 'minecraft:end_stone_brick_wall', + 'minecraft:diorite_wall', + +] + +NON_SOLID = [ + 'minecraft:air', + 'minecraft:powered_rail', + 'minecraft:detector_rail', + 'minecraft:grass', + 'minecraft:fern', + 'minecraft:dead_bush', + 'minecraft:seagrass', + 'minecraft:tall_seagrass', + 'minecraft:dandelion', + 'minecraft:poppy', + 'minecraft:blue_orchid', + 'minecraft:allium', + 'minecraft:azure_bluet', + 'minecraft:red_tulip', + 'minecraft:orange_tulip', + 'minecraft:white_tulip', + 'minecraft:pink_tulip', + 'minecraft:oxeye_daisy', + 'minecraft:cornflower', + 'minecraft:wither_rose', + 'minecraft:lily_of_the_valley', + 'minecraft:brown_mushroom', + 'minecraft:red_mushroom', + 'minecraft:torch', + 'minecraft:wall_torch', + 'minecraft:redstone_wire', + 'minecraft:wheat', + 'minecraft:oak_sign', + 'minecraft:spruce_sign', + 'minecraft:birch_sign', + 'minecraft:acacia_sign', + 'minecraft:jungle_sign', + 'minecraft:dark_oak_sign', + 'minecraft:rail', + 'minecraft:oak_wall_sign', + 'minecraft:spruce_wall_sign', + 'minecraft:birch_wall_sign', + 'minecraft:acacia_wall_sign', + 'minecraft:jungle_wall_sign', + 'minecraft:dark_oak_wall_sign', + 'minecraft:lever', + 'minecraft:stone_pressure_plate', + 'minecraft:oak_pressure_plate', + 'minecraft:spruce_pressure_plate', + 'minecraft:birch_pressure_plate', + 'minecraft:jungle_pressure_plate', + 'minecraft:acacia_pressure_plate', + 'minecraft:dark_oak_pressure_plate', + 'minecraft:redstone_torch', + 'minecraft:redstone_wall_torch', + 'minecraft:stone_button', + 'minecraft:sugar_cane', + 'minecraft:repeater', + 'minecraft:attached_pumpkin_stem', + 'minecraft:attached_melon_stem', + 'minecraft:pumpkin_stem', + 'minecraft:melon_stem', + 'minecraft:nether_wart', + 'minecraft:tripwire_hook', + 'minecraft:tripwire', + 'minecraft:carrots', + 'minecraft:potatoes', + 'minecraft:oak_button', + 'minecraft:spruce_button', + 'minecraft:birch_button', + 'minecraft:jungle_button', + 'minecraft:acacia_button', + 'minecraft:dark_oak_button', + 'minecraft:light_weighted_pressure_plate', + 'minecraft:heavy_weighted_pressure_plate', + 'minecraft:comparator', + 'minecraft:activator_rail', + 'minecraft:white_carpet', + 'minecraft:orange_carpet', + 'minecraft:magenta_carpet', + 'minecraft:light_blue_carpet', + 'minecraft:yellow_carpet', + 'minecraft:lime_carpet', + 'minecraft:pink_carpet', + 'minecraft:gray_carpet', + 'minecraft:light_gray_carpet', + 'minecraft:cyan_carpet', + 'minecraft:purple_carpet', + 'minecraft:blue_carpet', + 'minecraft:brown_carpet', + 'minecraft:green_carpet', + 'minecraft:red_carpet', + 'minecraft:black_carpet', + 'minecraft:sunflower', + 'minecraft:lilac', + 'minecraft:rose_bush', + 'minecraft:peony', + 'minecraft:tall_grass', + 'minecraft:large_fern', + 'minecraft:white_banner', + 'minecraft:orange_banner', + 'minecraft:magenta_banner', + 'minecraft:light_blue_banner', + 'minecraft:yellow_banner', + 'minecraft:lime_banner', + 'minecraft:pink_banner', + 'minecraft:gray_banner', + 'minecraft:light_gray_banner', + 'minecraft:cyan_banner', + 'minecraft:purple_banner', + 'minecraft:blue_banner', + 'minecraft:brown_banner', + 'minecraft:green_banner', + 'minecraft:red_banner', + 'minecraft:black_banner', + 'minecraft:white_wall_banner', + 'minecraft:orange_wall_banner', + 'minecraft:magenta_wall_banner', + 'minecraft:light_blue_wall_banner', + 'minecraft:yellow_wall_banner', + 'minecraft:lime_wall_banner', + 'minecraft:pink_wall_banner', + 'minecraft:gray_wall_banner', + 'minecraft:light_gray_wall_banner', + 'minecraft:cyan_wall_banner', + 'minecraft:purple_wall_banner', + 'minecraft:blue_wall_banner', + 'minecraft:brown_wall_banner', + 'minecraft:green_wall_banner', + 'minecraft:red_wall_banner', + 'minecraft:black_wall_banner', + 'minecraft:beetroots', + 'minecraft:bamboo_sapling', + 'minecraft:void_air', + 'minecraft:cave_air', + 'minecraft:lantern', +] +SINGLE_SNOW = 3919 + +LOGS = [ + 'minecraft:oak_log', + 'minecraft:spruce_log', + 'minecraft:birch_log', + 'minecraft:jungle_log', + 'minecraft:acacia_log', + 'minecraft:dark_oak_log', +] + +LEAVES = [ + 'minecraft:oak_leaves', + 'minecraft:spruce_leaves', + 'minecraft:birch_leaves', + 'minecraft:jungle_leaves', + 'minecraft:acacia_leaves', + 'minecraft:dark_oak_leaves', +] + + +NON_SOLID_IDS = set([SINGLE_SNOW]) +for block_name in NON_SOLID: + for state in BLOCKS[block_name]['states']: + NON_SOLID_IDS.add(state['id']) + +AVOID_IDS = set() +for block_name in AVOID: + for state in BLOCKS[block_name]['states']: + AVOID_IDS.add(state['id']) + +LOG_IDS = set() +for block_name in LOGS: + for state in BLOCKS[block_name]['states']: + LOG_IDS.add(state['id']) + +LEAF_IDS = set() +for block_name in LEAVES: + for state in BLOCKS[block_name]['states']: + LEAF_IDS.add(state['id']) diff --git a/bot.py b/bot.py index be23708..990fe6b 100644 --- a/bot.py +++ b/bot.py @@ -5,6 +5,7 @@ if __name__ == '__main__': import os import time import importlib +from math import floor, ceil USERNAME = os.environ['USERNAME'] PASSWORD = os.environ['PASSWORD'] @@ -21,19 +22,144 @@ from minecraft.networking.packets import Packet, clientbound, serverbound from custom.networking.packets.clientbound.play.block_change_packet import BlockChangePacket -from panda3d.core import LPoint3f +from bunch import Bunch +from panda3d.core import LPoint3f, LVector3f import packet_handlers importlib.reload(packet_handlers) +import blocks +importlib.reload(blocks) +import utils +importlib.reload(utils) +import path +importlib.reload(path) TICK = 0.05 last_tick = time.time() -def tick(): - return +PITCH_ANGLE_DIR = LVector3f(x=0, y=1, z=0) +YAW_ANGLE_DIR = LVector3f(x=0, y=0, z=-1) +YAW_ANGLE_REF = LVector3f(x=0, y=1, z=0) +YAW_LOOK_AHEAD = 4 + + + +def tick(global_state): + g = global_state + l = g.local_state + p = g.pos + + target = None + + try: + g.chunks.get_block_at(*utils.pint(p)) + except chunks.ChunkNotLoadedException: + return + + #l.jobstate.run() + + if l.path and len(l.path): + target = LPoint3f(l.path[0]) + target.x += 0.5 + target.z += 0.5 + + if target: + d = p - target + + # jump up block + if d.y < -0.9 and not l.y_v: + l.y_v = 8.5 + l.y_a = -36.0 + + # jump gap + if d.xz.length() > 1.6 and not l.y_v: + l.y_v = 8.5 + l.y_a = -36.0 + + if d.length() > 0: + if l.y_v < 5: + p.x -= utils.cap(d.x, 0.2) + p.z -= utils.cap(d.z, 0.2) + + if len(l.path) > 1 and d.length() < 0.2: + # removes some jitter in walking + l.path.pop(0) + elif d.length() == 0: + l.path.pop(0) + + if l.y_v or l.y_a: + p.y += l.y_v * TICK + l.y_v += l.y_a * TICK + + block_below = g.chunks.get_block_at(floor(p.x), ceil(p.y-1), floor(p.z)) + in_air = block_below in blocks.NON_SOLID_IDS + + if in_air: + l.y_a = -36.0 + else: + p.y = ceil(p.y) + l.y_v = 0 + l.y_a = 0 + + if l.look_at: + look_at = LPoint3f(l.look_at) + elif l.path and len(l.path) > YAW_LOOK_AHEAD: + look_at = LPoint3f(l.path[YAW_LOOK_AHEAD]) + elif l.path and len(l.path): + look_at = LPoint3f(l.path[-1]) + else: + look_at = None + + if look_at: + look_at.x += 0.5 + look_at.z += 0.5 + look_at_d = p - look_at + + if look_at_d.length() > 0.6: + target_pitch = look_at_d.normalized().angleDeg(PITCH_ANGLE_DIR) + target_pitch = (target_pitch - 90) * -1 + target_pitch_d = target_pitch - l.pitch + l.pitch += utils.cap(target_pitch_d, 10) + + # remove vertical component for yaw calculation + look_at_d.y = 0 + + target_yaw = look_at_d.normalized().signedAngleDeg(other=YAW_ANGLE_DIR, ref=YAW_ANGLE_REF) + target_yaw_d = target_yaw - l.yaw + target_yaw_d = (target_yaw_d + 180) % 360 - 180 + l.yaw += utils.cap(target_yaw_d, 30) + else: + target_pitch_d = 0 - l.pitch + l.pitch += utils.cap(target_pitch_d, 10) + + packet = serverbound.play.PositionAndLookPacket(x=p.x, feet_y=p.y, z=p.z, pitch=l.pitch, yaw=l.yaw, on_ground=(not in_air)) + g.connection.write_packet(packet, force=True) + + +def init(global_state): + g = global_state + l = g.local_state + + l.time = 0 + + l.path = [] + l.look_at = None + l.y_v = 0 + l.y_a = 0 + l.yaw = 360 + l.pitch = 0 + + #l.break = None + #l.break_time = 0 + #l.break_timeout = 0 + #l.break_finished_packet = None + + #l.jobstate = JobStates(connection, player_info) + #l.jobstate.run() def bot(global_state): g = global_state + g.local_state = Bunch() if 'mcdata' not in g: g.mcdata = DataManager('./mcdata') @@ -58,31 +184,35 @@ def bot(global_state): def packet_wrapper(handler): def wrapper(packet): - print('called') + print('Wrapper:', handler) handler(packet, g) return wrapper - h = packet_wrapper(packet_handlers.handle_join_game) - g.connection.register_packet_listener(h, clientbound.play.JoinGamePacket) + h1 = packet_wrapper(packet_handlers.handle_join_game) + g.connection.register_packet_listener(h1, clientbound.play.JoinGamePacket) - h = packet_wrapper(packet_handlers.handle_block_change) - g.connection.register_packet_listener(h, BlockChangePacket) + h2 = packet_wrapper(packet_handlers.handle_position_and_look) + g.connection.register_packet_listener(h2, clientbound.play.PlayerPositionAndLookPacket) - h = packet_wrapper(packet_handlers.handle_position_and_look) - g.connection.register_packet_listener(h, clientbound.play.PlayerPositionAndLookPacket) + h3 = packet_wrapper(packet_handlers.handle_block_change) + g.connection.register_packet_listener(h3, BlockChangePacket) try: - #while not player_info.pos: - # time.sleep(TICK) - #print('Player loaded.') + while not g.pos: + time.sleep(TICK) + print('Player loaded.') + + x, y, z = utils.pint(g.pos) + while (floor(x/16), floor(y/16), floor(z/16)) not in g.chunks.chunks: + time.sleep(TICK) + print('Chunks loaded.') - #x, y, z = pint(player_info.pos) - #while (floor(x/16), floor(y/16), floor(z/16)) not in player_info.chunks.chunks: - # time.sleep(TICK) - #print('Chunks loaded.') + print('init..') + init(g) + print('done init') while g.running: - tick() + tick(g) global last_tick sleep_time = TICK + last_tick - time.time() diff --git a/main.py b/main.py index 1071f9a..7e894cf 100644 --- a/main.py +++ b/main.py @@ -10,9 +10,12 @@ from watchdog.observers import Observer from watchdog.events import PatternMatchingEventHandler import bot + global_state = Bunch() g = global_state +g.local_state = False g.connection = False +g.pos = False @app.route('/') def hello_world(): diff --git a/packet_handlers.py b/packet_handlers.py index d37f21f..39b7824 100644 --- a/packet_handlers.py +++ b/packet_handlers.py @@ -1,4 +1,12 @@ -from panda3d.core import * +import time +import importlib + +from panda3d.core import LPoint3f + +import utils +importlib.reload(utils) +import path +importlib.reload(path) def handle_join_game(packet, g): print('Connected.') @@ -6,9 +14,34 @@ def handle_join_game(packet, g): g.info = packet def handle_block_change(packet, g): + l = g.local_state print('block change:') print(packet) + if packet.block_state_id == 3887: + try: + l.goal = LPoint3f(x=packet.location[0], y=packet.location[1], z=packet.location[2]) + print('new waypoint:', l.goal) + + start = time.time() + solution = path.Pathfinder(g.chunks).astar(utils.pint(g.pos), utils.pint(l.goal)) + if solution: + solution = list(solution) + l.path = solution + #l.jobstate.state = l.jobstate.stop + print(len(solution)) + print(solution) + print(round(time.time() - start, 3), 'seconds') + else: + print('No path found') + #say(connection, 'No path found') + + #l.y_v = 10.0 + #l.y_a = -36.0 + except BaseException as e: + import traceback + print(traceback.format_exc()) + def handle_position_and_look(packet, g): print('pos and look:') print(packet) diff --git a/path.py b/path.py new file mode 100644 index 0000000..deff7f0 --- /dev/null +++ b/path.py @@ -0,0 +1,296 @@ +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) diff --git a/utils.py b/utils.py new file mode 100644 index 0000000..a4cbc19 --- /dev/null +++ b/utils.py @@ -0,0 +1,27 @@ +from math import floor, ceil + +def padd(p1, p2): + return (p1[0] + p2[0], p1[1] + p2[1], p1[2] + p2[2]) + +def psub(p1, p2): + return (p1[0] - p2[0], p1[1] - p2[1], p1[2] - p2[2]) + +def pmul(p, s): + return (s*p[0], s*p[1], s*p[2]) + +def phyp(p1, p2): + return hypot(p1[0] - p2[0], p1[1] - p2[1], p1[2] - p2[2]) + +def phyp_bias(p1, p2, origin): + origin_distance = phyp(origin, p2) + height_diff = p2[1] - p1[1] + height_diff = height_diff*8 if height_diff < 0 else height_diff*0.5 + return hypot(p1[0] - p2[0], height_diff, p1[2] - p2[2]) + origin_distance*1.5 + +def pint(p): + return (floor(p[0]), floor(p[1]), floor(p[2])) + +def cap(x, amount): + sign = 1 if x >= 0 else -1 + return sign * min(abs(x), amount) +