Fix yaw spinning bug and optimize pathfinding more

bot.py

@@ -30,6 +30,9 @@ from astar import AStar
 
 
 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)
 
 TRAVERSE_NORTH = (0, 0, -1)
@@ -111,6 +114,13 @@ PARKOUR = [
     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,
@@ -186,27 +196,30 @@ class MazeSolver(AStar):
         if not self.bair(padd(thru1, BLOCK_ABOVE)):
             return False
 
+        if self.bavoid(padd(thru1, BLOCK_BELOW)):
+            return False
+
         if not self.bair(thru2):
             return False
 
         if not self.bair(padd(thru2, BLOCK_ABOVE)):
             return False
 
+        if self.bavoid(padd(thru2, BLOCK_BELOW)):
+            return False
+
         return True
 
     def check_ascend(self, node, offset):
         if not self.check_traverse(node, offset):
             return False
 
-        head = padd(node, BLOCK_ABOVE)
-
         dest = padd(node, offset)
-        dest_head = padd(dest, BLOCK_ABOVE)
 
-        if not self.bair(padd(head, BLOCK_ABOVE)):
+        if not self.bair(padd(node, BLOCK_ABOVE2)):
             return False
 
-        if not self.bair(padd(dest_head, BLOCK_ABOVE)):
+        if not self.bair(padd(dest, BLOCK_ABOVE2)):
             return False
 
         return True
@@ -216,9 +229,8 @@ class MazeSolver(AStar):
             return False
 
         dest = padd(node, offset)
-        dest_head = padd(dest, BLOCK_ABOVE)
 
-        if not self.bair(padd(dest_head, BLOCK_ABOVE)):
+        if not self.bair(padd(dest, BLOCK_ABOVE2)):
             return False
 
         return True
@@ -228,10 +240,8 @@ class MazeSolver(AStar):
             return False
 
         dest = padd(node, offset)
-        dest_head = padd(dest, BLOCK_ABOVE)
-        dest_head_above = padd(dest_head, BLOCK_ABOVE)
 
-        if not self.bair(padd(dest_head_above, BLOCK_ABOVE)):
+        if not self.bair(padd(dest, BLOCK_ABOVE3)):
             return False
 
         return True
@@ -241,18 +251,15 @@ class MazeSolver(AStar):
             return False
 
         dest = padd(node, offset)
-        dest_head = padd(dest, BLOCK_ABOVE)
-        dest_head_above = padd(dest_head, BLOCK_ABOVE)
-        dest_head_above_above = padd(dest_head_above, BLOCK_ABOVE)
 
-        if not self.bair(padd(dest_head_above_above, BLOCK_ABOVE)):
+        if not self.bair(padd(dest, BLOCK_ABOVE4)):
             return False
 
         return True
 
     def check_parkour(self, node, offset):
         dest = padd(node, offset)
-        half_offset = (int(offset[0]/2), offset[1], int(offset[2]/2))
+        half_offset = HALF_PARKOUR[offset]
         middle = padd(node, half_offset)
 
         # dont jump if we can walk instead
@@ -262,12 +269,10 @@ class MazeSolver(AStar):
         if not self.check_ascend(node, offset):
             return False
 
-        middle_head = padd(middle, BLOCK_ABOVE)
-
-        if not self.bair(middle_head):
+        if not self.bair(padd(middle, BLOCK_ABOVE)):
             return False
 
-        if not self.bair(padd(middle_head, BLOCK_ABOVE)):
+        if not self.bair(padd(middle, BLOCK_ABOVE2)):
             return False
 
         return True
@@ -391,12 +396,9 @@ def tick(connection, player_info):
 
         if look_at_d.length() > 0.6:
             target_yaw = look_at_d.normalized().signedAngleDeg(other=ANGLE_DIR, ref=ANGLE_REF)
-            target_yaw_d = s['yaw'] - target_yaw
-            #print('target', target_yaw, 'd', target_yaw_d)
-            #if target_yaw_d > 270:
-            #    target_yaw += 360
-            #target_yaw_d = s['yaw'] - target_yaw
-            s['yaw'] -= cap(target_yaw_d, 50)
+            target_yaw_d = target_yaw - s['yaw']
+            target_yaw_d = (target_yaw_d + 180) % 360 - 180
+            s['yaw'] += cap(target_yaw_d, 30)
 
 
 
@@ -442,7 +444,7 @@ def main(connection, player_info):
                 solution = MazeSolver(player_info.chunks).astar(pint(player_info.pos), pint(s['goal']))
                 if solution:
                     solution = list(solution)
-                    s['path'] = solution
+                    #s['path'] = solution
                     print(len(solution))
                     print(round(time.time() - start, 3), 'seconds')
                 else:
@@ -482,6 +484,19 @@ def main(connection, player_info):
             packet = serverbound.play.ChatPacket()
             packet.message = str(block)
             connection.write_packet(packet)
+        elif '!path' in chat_packet.json_data:
+            try:
+                s['goal'] = LPoint3f(655, 86, 341)
+                print('new waypoint:', s['goal'])
+                start = time.time()
+                solution = MazeSolver(player_info.chunks).astar(pint(player_info.pos), pint(s['goal']))
+                solution = list(solution)
+                #s['path'] = solution
+                print(len(solution))
+                print(round(time.time() - start, 3), 'seconds')
+            except BaseException as e:
+                import traceback
+                print(traceback.format_exc())
 	
 
     connection.register_packet_listener(