Handle door navigation and turnarounds

mosfet/bot.py

@@ -79,6 +79,18 @@ def update_pitch(g, look_at_d):
     g.pitch += utils.cap(target_pitch - g.pitch, 10)
 
 
+def update_yaw(g, look_at_d, max_turn=30, fixed_target=None):
+    if fixed_target is not None:
+        g.yaw += utils.cap(fixed_target - g.yaw, max_turn)
+        return
+    if look_at_d.length() <= 0.6:
+        return
+    target_yaw = look_at_d.normalized().signedAngleDeg(
+        other=YAW_ANGLE_DIR, ref=YAW_ANGLE_REF)
+    delta = (target_yaw - g.yaw + 180) % 360 - 180
+    g.yaw = (g.yaw + utils.cap(delta, max_turn) + 180) % 360 - 180
+
+
 def missing_movement_chunks(g):
     """Return chunks needed for local physics and the next path steps only."""
     position = g.get('pos')
@@ -142,6 +154,10 @@ def tick(global_state):
             print('[movement] status={}'.format(status), flush=True)
         g.movement_status = status
 
+    door_pause = (g.game.handle_path_doors()
+                  if g.get('game') and
+                  hasattr(g.game, 'handle_path_doors') else False)
+
     ########## object physics ##########
     # note: it's possible the chunk data is out of date when this runs
 
@@ -176,7 +192,7 @@ def tick(global_state):
 
     ########## player physics ##########
 
-    if g.path and len(g.path):
+    if g.path and len(g.path) and not door_pause:
         target = vector.Point3D(g.path[0])
         target.x += 0.5
         target.z += 0.5
@@ -247,14 +263,13 @@ def tick(global_state):
 
         # remove vertical component for yaw calculation
         look_at_d.y = 0
-
+        update_yaw(g, look_at_d,
-        if look_at_d.length() > 0.6:
+                   fixed_target=g.get('yaw_target_override'))
-            target_yaw = look_at_d.normalized().signedAngleDeg(other=YAW_ANGLE_DIR, ref=YAW_ANGLE_REF)
-            target_yaw_d = target_yaw - g.yaw
-            target_yaw_d = (target_yaw_d + 180) % 360 - 180
-            g.yaw += utils.cap(target_yaw_d, 30)
     else:
         update_pitch(g, None)
+        if g.get('yaw_target_override') is not None:
+            update_yaw(g, None,
+                       fixed_target=g.yaw_target_override)
 
     from minecraft.networking.modern import (
         ClientTickEnd, PlayerPositionRotation,

mosfet/game.py

@@ -666,6 +666,8 @@ class LegacyGame:
 # migrated actions use the 26.3 packet classes imported above.
 class Game(LegacyGame):
     ARM_SWING_INTERVAL = 0.1
+    DOOR_AUTO_OPEN_GRACE = 0.25
+    DOOR_TURN_DURATION = 0.3
 
     def __init__(self, global_state):
         self.g = global_state
@@ -673,6 +675,9 @@ class Game(LegacyGame):
         self.break_deadline = None
         self.breaking_state = None
         self.next_swing_at = 0.0
+        self.door_navigation = None
+        self.g.yaw_target_override = None
+        self.g.unopenable_doors = self.g.get('unopenable_doors') or set()
 
         from mosfet.protocol.packets import SetHealthPacket
         # Health is a protocol-777 packet, not the legacy UpdateHealthPacket.
@@ -725,6 +730,258 @@ class Game(LegacyGame):
         self.sequence += 1
         return self.sequence
 
+    def _door_info(self, position):
+        state_id = self.g.world.block_at(*position)
+        if state_id not in blocks.DOOR_IDS:
+            return None
+        properties = blocks.PROPS.get(state_id, {})
+        lower = tuple(position)
+        if properties.get('half') == 'upper':
+            lower = utils.padd(lower, path.BLOCK_BELOW)
+            lower_state = self.g.world.block_at(*lower)
+            if lower_state in blocks.DOOR_IDS:
+                state_id = lower_state
+                properties = blocks.PROPS.get(state_id, {})
+        return lower, state_id, properties
+
+    def _toggle_door(self, lower, state_id):
+        position = utils.pint(self.g.pos)
+        dx, dz = position[0] - lower[0], position[2] - lower[2]
+        if abs(dx) > abs(dz):
+            if dx >= 0:
+                face, cursor = BlockFace.EAST, (0.95, 0.5, 0.5)
+            else:
+                face, cursor = BlockFace.WEST, (0.05, 0.5, 0.5)
+        elif abs(dz) > 0:
+            if dz >= 0:
+                face, cursor = BlockFace.SOUTH, (0.5, 0.5, 0.95)
+            else:
+                face, cursor = BlockFace.NORTH, (0.5, 0.5, 0.05)
+        else:
+            face, cursor = BlockFace.TOP, (0.5, 0.95, 0.5)
+
+        boxes = blocks.COLLISION_BOXES.get(state_id, ())
+        if boxes:
+            box = boxes[0]
+            cursor = ((box[0] + box[3]) / 2,
+                      (box[1] + box[4]) / 2,
+                      (box[2] + box[5]) / 2)
+        self.place_block(lower, face, cursor=cursor)
+
+    def _look_at_with_yaw_separation(self, target, fallback=None):
+        """Keep target yaw far enough from the player to avoid the near-zero guard."""
+        player = self.g.pos
+        player_x = player.x if hasattr(player, 'x') else player[0]
+        player_z = player.z if hasattr(player, 'z') else player[2]
+        center = [target[0] + 0.5, target[1] + 0.5, target[2] + 0.5]
+        dx, dz = player_x - center[0], player_z - center[2]
+        distance = utils.hypot(dx, dz)
+        if distance < 0.8:
+            if distance < 1e-6 and fallback is not None:
+                dx, dz = fallback
+                distance = utils.hypot(dx, dz)
+            if distance > 1e-6:
+                scale = 0.8 / distance
+                center[0] = player_x - dx * scale
+                center[2] = player_z - dz * scale
+        return tuple(value - 0.5 for value in center)
+
+    def _door_yaw_target(self, look_at):
+        target = vector.Point3D(look_at)
+        target.x += 0.5
+        target.z += 0.5
+        position = self.g.pos
+        if not hasattr(position, 'x'):
+            position = vector.Point3D(position)
+        direction = position - target
+        direction.y = 0
+        if direction.length() <= 0.6:
+            return self.g.yaw
+        return direction.normalized().signedAngleDeg(
+            other=bot.YAW_ANGLE_DIR, ref=bot.YAW_ANGLE_REF)
+
+    def _begin_door_turn(self, navigation, target, fallback=None):
+        look_at = self._look_at_with_yaw_separation(target, fallback)
+        yaw = self.g.yaw
+        target_yaw = self._door_yaw_target(look_at)
+        delta = (target_yaw - yaw + 180) % 360 - 180
+        navigation['yaw_origin'] = yaw
+        navigation['yaw_door_target'] = yaw + delta
+        self.g.yaw_target_override = navigation['yaw_door_target']
+        self.g.look_at = look_at
+
+    def _begin_door_return(self, navigation, now):
+        navigation['phase'] = 'turning_back'
+        navigation['deadline'] = now + self.DOOR_TURN_DURATION
+        self.g.yaw_target_override = navigation.get('yaw_origin')
+        target = navigation.get('forward_look_at')
+        self.g.look_at = (self._look_at_with_yaw_separation(target)
+                          if target is not None else
+                          navigation['previous_look_at'])
+
+    def _restore_door_look(self, navigation):
+        self.g.look_at = navigation['previous_look_at']
+        self.g.yaw_target_override = None
+        self.door_navigation = None
+
+    def handle_path_doors(self):
+        """Open closed wooden doors in the active route and close them after crossing."""
+        now = time.monotonic()
+        navigation = self.door_navigation
+        if navigation is not None:
+            lower = navigation['position']
+            info = self._door_info(lower)
+            is_open = bool(info and info[2].get('open') == 'true')
+
+            if navigation['phase'] == 'checking':
+                if (info is None or
+                        info[1] not in blocks.OPENABLE_DOOR_IDS):
+                    self._restore_door_look(navigation)
+                    return False
+                if is_open:
+                    navigation['phase'] = 'crossing'
+                    navigation['opened_by_bot'] = False
+                    navigation['saw_open'] = True
+                    self.g.look_at = navigation['previous_look_at']
+                    return False
+                if now >= navigation['deadline']:
+                    navigation['phase'] = 'opening'
+                    navigation['opened_by_bot'] = True
+                    navigation['deadline'] = now + 2.0
+                    print('[door] opening on path at', lower, flush=True)
+                    self._toggle_door(lower, info[1])
+                return True
+
+            if navigation['phase'] == 'opening':
+                if not self.g.path:
+                    self._restore_door_look(navigation)
+                    return False
+                if is_open:
+                    navigation['phase'] = 'crossing'
+                    navigation['saw_open'] = True
+                    self.g.look_at = navigation['previous_look_at']
+                    return False
+                if now > navigation['deadline']:
+                    upper = utils.padd(lower, path.BLOCK_ABOVE)
+                    self.g.unopenable_doors.update((lower, upper))
+                    goal = navigation['goal']
+                    self._restore_door_look(navigation)
+                    current = utils.pint(self.g.pos)
+                    self.g.path = (self.g.world.path_to_place(current, goal)
+                                   or [])
+                    print('Door did not open; routing around', lower,
+                          flush=True)
+                    return False
+                return True
+
+            if navigation['phase'] == 'crossing':
+                if navigation['after'] is not None and \
+                        utils.pint(self.g.pos) == navigation['after']:
+                    if navigation.get('saw_open', False):
+                        navigation['phase'] = 'turning_to_close'
+                        navigation['deadline'] = (
+                            now + self.DOOR_TURN_DURATION)
+                        navigation['close_door'] = (
+                            navigation.get('opened_by_bot', False) and
+                            is_open and info is not None and
+                            info[2].get('powered') != 'true')
+                        after = navigation['after']
+                        fallback = ((after[0] - lower[0],
+                                     after[2] - lower[2])
+                                    if after is not None else None)
+                        self._begin_door_turn(navigation, lower, fallback)
+                        return True
+                    self._restore_door_look(navigation)
+                    return False
+                if not is_open:
+                    # If a pressure plate or redstone opened it, wait briefly
+                    # for its state update rather than toggling it closed.
+                    if (info is not None and
+                            info[1] in blocks.OPENABLE_DOOR_IDS and
+                            lower in self.g.path):
+                        navigation['phase'] = 'checking'
+                        navigation['opened_by_bot'] = False
+                        navigation['deadline'] = (
+                            now + self.DOOR_AUTO_OPEN_GRACE)
+                        return True
+                    if (navigation.get('saw_open') and
+                            navigation['after'] is not None):
+                        return False
+                    self._restore_door_look(navigation)
+                return False
+
+            if navigation['phase'] == 'turning_to_close':
+                if now >= navigation['deadline']:
+                    if (navigation.get('close_door') and is_open and
+                            info is not None and
+                            info[2].get('powered') != 'true'):
+                        navigation['phase'] = 'closing'
+                        navigation['deadline'] = now + 2.0
+                        print('[door] closing after crossing at', lower,
+                              flush=True)
+                        self._toggle_door(lower, info[1])
+                    else:
+                        self._begin_door_return(navigation, now)
+                return True
+
+            if navigation['phase'] == 'closing':
+                if not is_open or now > navigation['deadline']:
+                    self._begin_door_return(navigation, now)
+                return True
+
+            if navigation['phase'] == 'turning_back':
+                if now >= navigation['deadline']:
+                    self._restore_door_look(navigation)
+                    return False
+                return True
+
+        if not self.g.path:
+            return False
+
+        current = utils.pint(self.g.pos)
+        path_index = 0
+        if (len(self.g.path) > 1 and
+                tuple(self.g.path[0]) == current):
+            path_index = 1
+        if path_index >= len(self.g.path):
+            return False
+        info = self._door_info(tuple(self.g.path[path_index]))
+        if info is None:
+            return False
+        lower, state_id, properties = info
+        is_open = properties.get('open') == 'true'
+        if (not is_open and state_id not in blocks.OPENABLE_DOOR_IDS) or \
+                utils.phyp(current, lower) > 3:
+            return False
+
+        after_index = path_index + 1
+        after = (tuple(self.g.path[after_index])
+                 if after_index < len(self.g.path) else None)
+        if after_index + 1 < len(self.g.path):
+            forward_look_at = tuple(self.g.path[after_index + 1])
+        elif after is not None:
+            forward_look_at = tuple(
+                after[i] + 2 * (after[i] - lower[i]) for i in range(3))
+        else:
+            forward_look_at = None
+        navigation = {
+            'position': lower,
+            'after': after,
+            'goal': tuple(self.g.path[-1]),
+            'phase': 'crossing' if is_open else 'checking',
+            'deadline': now + self.DOOR_AUTO_OPEN_GRACE,
+            'opened_by_bot': False,
+            'saw_open': is_open,
+            'close_door': False,
+            'previous_look_at': self.g.get('look_at'),
+            'forward_look_at': forward_look_at,
+        }
+        self.door_navigation = navigation
+        if not is_open:
+            self.g.look_at = lower
+            return True
+        return False
+
     def animate(self):
         """Ask the 26.3 server to broadcast a main-hand punch animation."""
         self._send_action(PunchPacket())

mosfet/info/blocks.py

@@ -86,6 +86,11 @@ LEAF_IDS = get_set(LEAVES)
 CHEST_IDS = get_set(['chest'])
 BARREL_IDS = get_set(['barrel'])
 TRAPPED_CHEST_IDS = get_set(['trapped_chest'])
+DOOR_NAMES = sorted(name for name in MCD_BLOCKS
+                    if name.endswith('_door'))
+DOOR_IDS = get_set(DOOR_NAMES)
+OPENABLE_DOOR_IDS = get_set(
+    name for name in DOOR_NAMES if name != 'iron_door')
 NETHERWART_IDS = get_set(['nether_wart'])
 WHEAT_IDS = get_set(['wheat'])
 POTATO_IDS = get_set(['potatoes'])

mosfet/path.py

@@ -125,7 +125,13 @@ class Pathfinder(AStar):
 
     @functools.lru_cache(maxsize=BLOCK_CACHE_SIZE)
     def bair(self, p):
-        return self.chunks.get_block_at(*p) in blocks.NON_SOLID_IDS
+        state_id = self.chunks.get_block_at(*p)
+        if state_id in blocks.DOOR_IDS:
+            if blocks.PROPS.get(state_id, {}).get('open') == 'true':
+                return True
+            if state_id in blocks.OPENABLE_DOOR_IDS:
+                return tuple(p) not in self.g.get('unopenable_doors', set())
+        return state_id in blocks.NON_SOLID_IDS
 
     @functools.lru_cache(maxsize=BLOCK_CACHE_SIZE)
     def bavoid(self, p):

tests/test_door_navigation.py

@@ -0,0 +1,184 @@
+import unittest
+from munch import Munch
+
+from mosfet.game import Game
+from mosfet.info import blocks
+from mosfet.path import Pathfinder
+
+
+class FakeChunks:
+    def __init__(self, states):
+        self.states = states
+
+    def get_block_at(self, *position):
+        return self.states.get(tuple(position))
+
+
+class DoorNavigationTests(unittest.TestCase):
+    @staticmethod
+    def door_state(name, half='lower', opened=False, powered=False):
+        data = blocks.MCD_BLOCKS[name]
+        return next(
+            state for state in range(data['minStateId'], data['maxStateId'] + 1)
+            if blocks.PROPS[state].get('half') == half and
+            blocks.PROPS[state].get('open') == str(opened).lower() and
+            blocks.PROPS[state].get('powered') == str(powered).lower())
+
+    def test_pathfinder_treats_openable_doors_as_passable_but_not_iron(self):
+        wooden = self.door_state('oak_door')
+        iron = self.door_state('iron_door')
+        opened_iron = self.door_state('iron_door', opened=True)
+        positions = {(1, 64, 0): wooden, (2, 64, 0): iron,
+                     (3, 64, 0): opened_iron}
+        g = Munch(chunks=FakeChunks(positions), crawling=False,
+                  unopenable_doors=set())
+        finder = Pathfinder(g)
+
+        self.assertTrue(finder.bair((1, 64, 0)))
+        self.assertFalse(finder.bair((2, 64, 0)))
+        self.assertTrue(finder.bair((3, 64, 0)))
+
+        g.unopenable_doors.add((1, 64, 0))
+        finder = Pathfinder(g)
+        self.assertFalse(finder.bair((1, 64, 0)))
+
+    def test_pressure_plate_opened_door_is_not_toggled_or_closed(self):
+        door = (1, 64, 0)
+        after = (2, 64, 0)
+        closed = self.door_state('oak_door', powered=True)
+        opened = self.door_state('oak_door', opened=True, powered=True)
+        states = {door: closed}
+
+        class FakeWorld:
+            @staticmethod
+            def block_at(*position):
+                return states.get(tuple(position))
+
+        toggles = []
+        game = Game.__new__(Game)
+        game.g = Munch(
+            world=FakeWorld(), pos=(0.5, 64, 0.5), yaw=-90.0,
+            path=[door, after], unopenable_doors=set(), look_at=None)
+        game.door_navigation = None
+        game._toggle_door = lambda position, state: toggles.append(
+            (position, state))
+
+        self.assertTrue(game.handle_path_doors())
+        states[door] = opened  # the bot steps on a pressure plate
+        self.assertFalse(game.handle_path_doors())
+        self.assertEqual(game.door_navigation['phase'], 'crossing')
+        self.assertFalse(game.door_navigation['opened_by_bot'])
+
+        game.g.pos = (2.5, 64, 0.5)
+        self.assertTrue(game.handle_path_doors())
+        self.assertEqual(game.door_navigation['phase'], 'turning_to_close')
+        game.door_navigation['deadline'] = 0
+        self.assertTrue(game.handle_path_doors())
+        self.assertEqual(game.door_navigation['phase'], 'turning_back')
+        self.assertEqual(toggles, [])
+
+        states[door] = closed
+        game.door_navigation['deadline'] = 0
+        self.assertFalse(game.handle_path_doors())
+        self.assertIsNone(game.door_navigation)
+
+    def test_powered_door_is_left_to_pressure_plate_after_crossing(self):
+        door = (1, 64, 0)
+        after = (2, 64, 0)
+        closed = self.door_state('oak_door', powered=False)
+        powered_open = self.door_state('oak_door', opened=True, powered=True)
+        released_closed = self.door_state('oak_door', powered=False)
+        states = {door: closed}
+
+        class FakeWorld:
+            @staticmethod
+            def block_at(*position):
+                return states.get(tuple(position))
+
+        toggles = []
+        game = Game.__new__(Game)
+        game.g = Munch(
+            world=FakeWorld(), pos=(0.5, 64, 0.5), yaw=-90.0,
+            path=[door, after], unopenable_doors=set(), look_at=None)
+        game.door_navigation = None
+        game._toggle_door = lambda position, state: toggles.append(
+            (position, state))
+
+        game.handle_path_doors()
+        game.door_navigation['deadline'] = 0
+        game.handle_path_doors()
+        states[door] = powered_open
+        game.handle_path_doors()
+        game.g.pos = (2.5, 64, 0.5)
+        self.assertTrue(game.handle_path_doors())
+        self.assertEqual(game.door_navigation['phase'], 'turning_to_close')
+        game.door_navigation['deadline'] = 0
+        self.assertTrue(game.handle_path_doors())
+        self.assertEqual(game.door_navigation['phase'], 'turning_back')
+        self.assertEqual(toggles, [(door, closed)])
+
+        states[door] = released_closed
+        game.door_navigation['deadline'] = 0
+        self.assertFalse(game.handle_path_doors())
+        self.assertIsNone(game.door_navigation)
+
+    def test_opens_door_before_crossing_and_closes_it_after(self):
+        door = (1, 64, 0)
+        after = (2, 64, 0)
+        closed = self.door_state('oak_door', powered=False)
+        opened = self.door_state('oak_door', opened=True, powered=False)
+        states = {door: closed}
+
+        class FakeWorld:
+            @staticmethod
+            def block_at(*position):
+                return states.get(tuple(position))
+
+            @staticmethod
+            def path_to_place(_start, _goal):
+                return None
+
+        toggles = []
+        game = Game.__new__(Game)
+        game.g = Munch(
+            world=FakeWorld(), pos=(0.5, 64, 0.5), yaw=-90.0,
+            path=[door, after], unopenable_doors=set(), look_at=None)
+        game.door_navigation = None
+        game._toggle_door = lambda position, state: toggles.append(
+            (position, state))
+
+        self.assertTrue(game.handle_path_doors())
+        self.assertEqual(game.door_navigation['phase'], 'checking')
+        self.assertEqual(toggles, [])
+
+        game.door_navigation['deadline'] = 0
+        self.assertTrue(game.handle_path_doors())
+        self.assertEqual(game.door_navigation['phase'], 'opening')
+        self.assertEqual(toggles, [(door, closed)])
+
+        states[door] = opened
+        self.assertFalse(game.handle_path_doors())
+        self.assertEqual(game.door_navigation['phase'], 'crossing')
+
+        game.g.pos = (2.5, 64, 0.5)
+        self.assertTrue(game.handle_path_doors())
+        self.assertEqual(game.door_navigation['phase'], 'turning_to_close')
+        self.assertEqual(game.g.look_at, door)
+        self.assertEqual(len(toggles), 1)
+
+        game.door_navigation['deadline'] = 0
+        self.assertTrue(game.handle_path_doors())
+        self.assertEqual(game.door_navigation['phase'], 'closing')
+        self.assertEqual(toggles[-1], (door, opened))
+
+        states[door] = closed
+        self.assertTrue(game.handle_path_doors())
+        self.assertEqual(game.door_navigation['phase'], 'turning_back')
+        self.assertEqual(game.g.look_at, (4, 64, 0))
+        game.door_navigation['deadline'] = 0
+        self.assertFalse(game.handle_path_doors())
+        self.assertIsNone(game.door_navigation)
+
+
+if __name__ == '__main__':
+    unittest.main()

tests/test_farming_pitch.py

@@ -2,8 +2,8 @@ import unittest
 
 from munch import Munch
 
-from mosfet.bot import look_direction_from_eye, update_pitch
+from mosfet.bot import look_direction_from_eye, update_pitch, update_yaw
-from mosfet.vector import Point3D
+from mosfet.vector import Point3D, Vector3D
 
 
 class LookDirection:
@@ -38,6 +38,26 @@ class TestFarmingPitch(unittest.TestCase):
             update_pitch(state, directly_below)
         self.assertEqual(state.pitch, 45.0)
 
+    def test_fixed_yaw_turn_returns_over_the_same_180_degree_arc(self):
+        state = Munch(yaw=-90.0)
+        no_look_direction = Vector3D((0.0, 0.0, 0.0))
+
+        for _ in range(6):
+            update_yaw(state, no_look_direction, fixed_target=-270.0)
+        self.assertEqual(state.yaw, -270.0)
+
+        for _ in range(6):
+            update_yaw(state, no_look_direction, fixed_target=-90.0)
+        self.assertEqual(state.yaw, -90.0)
+
+    def test_yaw_turns_across_wrap_without_unbounded_rotation(self):
+        state = Munch(yaw=630.0)
+        update_yaw(state, Vector3D((0, 0, -1)))
+
+        self.assertEqual(state.yaw, -60.0)
+        self.assertGreaterEqual(state.yaw, -180.0)
+        self.assertLess(state.yaw, 180.0)
+
     def test_pitch_tracks_look_target_instead_of_a_fixed_farming_angle(self):
         state = Munch(pitch=0.0)
         direction_to_crop = LookDirection(45.0)