WIP Pathfinder

MineSharp.Bot.IntegrationTests/MineSharp.Bot.IntegrationTests.csproj

@@ -11,6 +11,7 @@
 
     <ItemGroup>
       <ProjectReference Include="..\MineSharp.Bot\MineSharp.Bot.csproj" />
+      <ProjectReference Include="..\Plugins\MineSharp.Pathfinder\MineSharp.Pathfinder.csproj" />
     </ItemGroup>
 
     <ItemGroup>

MineSharp.Bot.IntegrationTests/Program.cs

@@ -1,8 +1,125 @@
 using MineSharp.Bot;
 using MineSharp.Bot.IntegrationTests.Tests;
+using MineSharp.Bot.Plugins;
+using MineSharp.Core.Common;
+using MineSharp.Core.Common.Blocks;
+using MineSharp.Core.Common.Effects;
+using MineSharp.Core.Common.Entities;
+using MineSharp.Data;
+using MineSharp.Pathfinder;
+using MineSharp.Physics;
+using MineSharp.Physics.Input;
+using MineSharp.World;
+using MineSharp.World.Chunks;
 
-await PlayerTests.RunAll();
-await WindowTests.RunAll();
-await WorldTests.RunAll();
-await CraftingTests.RunAll();
-await PhysicsTests.RunAll();
+// Forward = True
+// Pos = (8,5 / 2 / 8,63005), Vel = (0 / -0,0784 / 0,07101) IsOnGround=False
+// Pos = (8,5 / 2 / 8,72065), Vel = (0 / -0,0784 / 0,08245) IsOnGround=True)
+// Pos = (8,5 / 2 / 8,93315), Vel = (0 / -0,0784 / 0,11602) IsOnGround=True)
+// Pos = (8,5 / 2 / 9,17922), Vel = (0 / -0,0784 / 0,13436) IsOnGround=True
+// Backward = True
+// Pos = (8,5 / 2 / 9,31358), Vel = (0 / -0,0784 / 0,07336) IsOnGround=True
+// Pos = (8,5 / 1,9216 / 9,38694), Vel = (0 / -0,15523 / 0,04005) IsOnGround=False
+// Backward = False
+// Forward = False
+// Pos = (8,5 / 1,76637 / 9,42699), Vel = (0 / -0,23053 / 0,03645) IsOnGround=False
+// Pos = (8,5 / 1,53584 / 9,46344), Vel = (0 / -0,30432 / 0,03317) IsOnGround=False
+// Pos = (8,5 / 1,23152 / 9,49661), Vel = (0 / -0,37663 / 0,03018) IsOnGround=False
+// Pos = (8,5 / 1 / 9,52679), Vel = (0 / -0,0784 / 0,02747) IsOnGround=True
+// Pos = (8,5 / 1 / 9,55426), Vel = (0 / -0,0784 / 0,015) IsOnGround=True
+// Pos = (8,5 / 1 / 9,56925), Vel = (0 / -0,0784 / 0,00819) IsOnGround=True
+
+
+var data = MinecraftData.FromVersion("1.20.2");
+var world = WorldVersion.CreateWorld(data);
+var chunk = world.CreateChunk(new ChunkCoordinates(0, 0), Array.Empty<BlockEntity>());
+
+var input = new InputControls();
+var entity = new Entity(
+    data.Entities.GetByType(EntityType.Player), 
+    0, 
+    new Vector3(8.5, 2, 8.5), 
+    0.0f, 
+    -45.0f, 
+    new Vector3(0, 0, 0), 
+    true, 
+    new Dictionary<EffectType, Effect?>());
+
+var minecraftPlayer = new MinecraftPlayer("yea", new UUID(), 0, GameMode.Survival, entity, Dimension.Overworld, PermissionLevel.Admin);
+
+var chunkBuffer = new List<byte>();
+for (int i = 0; i < 24; i++)
+    chunkBuffer.AddRange([0, 0, 0, 0, 0, 0, 0, 0]);
+
+chunk.LoadData(chunkBuffer.ToArray());
+world.LoadChunk(chunk);
+
+var stone = data.Blocks.GetByType(BlockType.Stone);
+world.SetBlock(new Block(stone, stone.DefaultState, new Position(8, 1, 8)));
+world.SetBlock(new Block(stone, stone.DefaultState, new Position(9, 1, 9)));
+
+var sim = new PlayerPhysics(data, minecraftPlayer, world, input);
+
+while (true)
+{
+    var key = Console.ReadKey();
+
+    if (key.Key == ConsoleKey.W)
+    {
+        input.ForwardKeyDown = !input.ForwardKeyDown;
+        Console.WriteLine($"Forward = {input.ForwardKeyDown}");
+        continue;
+    }
+
+    if (key.Key == ConsoleKey.S)
+    {
+        input.BackwardKeyDown = !input.BackwardKeyDown;
+        Console.WriteLine($"Backward = {input.BackwardKeyDown}");
+        continue;
+    }
+
+    sim.Tick();
+    Console.WriteLine($"Pos = {entity.Position}, Vel = {entity.Velocity} IsOnGround={entity.IsOnGround}");
+}
+
+// Console.ReadKey();
+//
+// var bot = await new BotBuilder()
+//     .Host("localhost")
+//     .OfflineSession("MineSharpBot")
+//     .WithPlugin<Pathfinder>()
+//     .WithPlugin<AutoRespawn>()
+//     .AutoConnect()
+//     .CreateAsync();
+//
+// await bot.Connect();
+//
+// var chat = bot.GetPlugin<ChatPlugin>();
+// var physics = bot.GetPlugin<PhysicsPlugin>();
+// var pathfinder = bot.GetPlugin<Pathfinder>();
+// var player = bot.GetPlugin<PlayerPlugin>();
+// var world = bot.GetPlugin<WorldPlugin>();
+//
+// if (!player.IsAlive!.Value)
+//     await player.Respawn();
+//
+// chat.OnChatMessageReceived += (sender, playerUuid, message, position, name) =>
+// {
+//     var splits = message.Message.Split(' ');
+//     
+//     if (splits.Length != 3)
+//         return;
+//
+//     var ints = splits.Select(x => Convert.ToInt32(x)).ToArray();
+//     pathfinder.Goto(new Position(ints[0], ints[1], ints[2])).Wait();
+// };
+//
+// Console.ReadKey();
+//
+// return;
+//
+// await PlayerTests.RunAll();
+// await WindowTests.RunAll();
+// await WorldTests.RunAll();
+// await CraftingTests.RunAll();
+// await PhysicsTests.RunAll();

MineSharp.Bot/Plugins/PhysicsPlugin.cs

@@ -256,7 +256,10 @@ public override Task OnTick()
         delta.Normalize();
 
         var yaw = deg2rad * Math.Atan2(-delta.X, delta.Z);
-        var pitch = deg2rad * -Math.Asin(delta.Y / delta.Length());
+        var pitch = deg2rad * -Math.Asin(delta.Y);
+
+        if (yaw < 0)
+            yaw += 360;
 
         return ((float)yaw, (float)pitch);
     }

MineSharp.Core/Common/Vector3.cs

@@ -250,10 +250,9 @@ public double Length()
     {
         return Math.Sqrt(this.LengthSquared());
     }
-
-
+
     /// <summary>
-    /// Returns the squared length of this vector instance
+    /// Returns the square length of this vector instance
     /// </summary>
     /// <returns></returns>
     [Pure]
@@ -262,6 +261,16 @@ public double LengthSquared()
         return this.X * this.X + this.Y * this.Y + this.Z * this.Z;
     }
 
+    /// <summary>
+    /// Returns the square horizontal length of this vector
+    /// </summary>
+    /// <returns></returns>
+    [Pure]
+    public double HorizontalLengthSquared()
+    {
+        return this.X * this.X + this.Z * this.Z;
+    }
+
     /// <summary>
     /// Returns the horizontal squared length of this vector
     /// </summary>

Plugins/MineSharp.Pathfinder/Movements.cs

@@ -45,6 +45,9 @@ public Movements(bool allowSprinting, bool allowJumping)
             moves.AddRange(Directions.Select(x => new JumpUpMove(x)));
             moves.AddRange(DiagonalDirections.Select(x => new JumpUpMove(x)));
         }
+
+        moves.AddRange(Directions.Select(x => new FallDownMove(x)));
+        moves.AddRange(DiagonalDirections.Select(x => new FallDownMove(x)));
 
         this.PossibleMoves = moves.ToArray();
     }

Plugins/MineSharp.Pathfinder/Moves/FallDownMove.cs

@@ -0,0 +1,93 @@
+using MineSharp.Bot;
+using MineSharp.Bot.Plugins;
+using MineSharp.Core.Common;
+using MineSharp.Data;
+using MineSharp.Pathfinder.Utils;
+using MineSharp.World;
+using MineSharp.World.Iterators;
+
+namespace MineSharp.Pathfinder.Moves;
+
+/// <summary>
+/// Fall down to an adjacent block 
+/// </summary>
+public class FallDownMove(Vector3 motion) : IMove
+{
+    /// <inheritdoc />
+    public Vector3 Motion { get; } = Vector3.Down.Plus(motion);
+
+    /// <inheritdoc />
+    public float Cost => 2;
+
+    /// <inheritdoc />
+    public bool CanBeLinked => false;
+
+    /// <inheritdoc />
+    public bool IsMovePossible(Position position, IWorld world, MinecraftData data)
+    {
+        var playerBb = CollisionHelper.GetPlayerBoundingBox(position);
+        playerBb.Offset(
+            0.5 + this.Motion.X / 2, 
+            -1, 
+            0.5 + this.Motion.Z / 2);
+
+        return new BoundingBoxIterator(playerBb)
+            .Iterate()
+            .Select(world.GetBlockAt)
+            .Select(x => CollisionHelper.GetBoundingBoxes(x, data))
+            .SelectMany(x => x)
+            .Any(x => x.Intersects(playerBb));    
+    }
+
+    public async Task PerformMove(MineSharpBot bot, int count, Movements movements)
+    {
+        var physics = bot.GetPlugin<PhysicsPlugin>();
+        var player = bot.GetPlugin<PlayerPlugin>();
+
+        var target = player.Self!.Entity!.Position
+            .Floored()
+            .Add(this.Motion)
+            .Add(0.5, 0.0, 0.5);
+
+        var targetEdge = target.Floored();
+        if (this.Motion.X != 0)
+            targetEdge.Add(GetBoundingBoxEdge(this.Motion.X), 0.0, 0.0);
+        if (this.Motion.Z != 0)
+            targetEdge.Add(0.0, 0.0, GetBoundingBoxEdge(this.Motion.Z));
+
+        await physics.LookAt(target);
+
+        var wasFalling = false;
+        var threshold = this.Motion.LengthSquared() * (0.05 * 0.05);
+
+        physics.InputControls.Reset();
+        physics.InputControls.ForwardKeyDown = true;
+        while (true)
+        {
+            await physics.WaitForTick();
+
+            if (!wasFalling && !player.Entity!.IsOnGround)
+            {
+                physics.InputControls.BackwardKeyDown = true;
+                wasFalling = true;
+            }
+
+            if (wasFalling && player.Entity!.Velocity.HorizontalLengthSquared() <= threshold)
+            {
+
+            }
+
+            // if (player.Entity.Velocity.)
+
+            var dst = target.DistanceToSquared(player.Self!.Entity!.Position);
+
+            if (dst <= IMove.THRESHOLD_COMPLETED)
+                break;
+        }
+
+        physics.InputControls.Reset();
+    }
+
+    private double GetBoundingBoxEdge(double axisMotion)
+        => this.Motion.X > 0 ? 0.3 : 0.7;
+}

Plugins/MineSharp.Pathfinder/Moves/JumpUpMove.cs

@@ -0,0 +1,95 @@
+using MineSharp.Bot;
+using MineSharp.Bot.Plugins;
+using MineSharp.Core.Common;
+using MineSharp.Data;
+using MineSharp.Pathfinder.Utils;
+using MineSharp.World;
+using MineSharp.World.Iterators;
+
+namespace MineSharp.Pathfinder.Moves;
+
+/// <summary>
+/// Jump up to an adjacent block
+/// </summary>
+/// <param name="xzMotion"></param>
+public class JumpUpMove(Vector3 xzMotion) : IMove
+{
+    /// <inheritdoc />
+    public Vector3 Motion { get; } = Vector3.Up.Plus(xzMotion);
+
+    /// <inheritdoc />
+    public float Cost => 20;
+
+    /// <inheritdoc />
+    public bool CanBeLinked => false;
+
+    /// <inheritdoc />
+    public bool IsMovePossible(Position position, IWorld world, MinecraftData data)
+    {
+        var playerBb = CollisionHelper.GetPlayerBoundingBox(position);
+        playerBb.Offset(
+            0.5 + this.Motion.X / 2, 
+            1, 
+            0.5 + this.Motion.Z / 2);
+
+        return new BoundingBoxIterator(playerBb)
+            .Iterate()
+            .Select(world.GetBlockAt)
+            .Select(x => CollisionHelper.GetBoundingBoxes(x, data))
+            .SelectMany(x => x)
+            .Any(x => x.Intersects(playerBb));
+    }
+
+
+    /// <inheritdoc />
+    public async Task PerformMove(MineSharpBot bot, int count, Movements movements)
+    {
+        if (count != 1)
+            throw new InvalidOperationException();
+
+        var physics = bot.GetPlugin<PhysicsPlugin>();
+        var player = bot.GetPlugin<PlayerPlugin>();
+
+        var target = player.Self!.Entity!.Position
+            .Floored()
+            .Add(this.Motion)
+            .Add(0.5, 0.0, 0.5);
+
+        await physics.LookAt(target);
+
+        physics.InputControls.ForwardKeyDown = true;
+        physics.InputControls.JumpingKeyDown = true;
+        physics.InputControls.SprintingKeyDown = movements.AllowSprinting && Math.Abs(this.Motion.LengthSquared() - 1) < 0.01;
+
+        var prevDst = double.MaxValue;
+
+        while (true)
+        {
+            await physics.WaitForTick();
+            if (!player.Entity!.IsOnGround)
+            {
+                physics.InputControls.JumpingKeyDown = false;
+                await physics.WaitForOnGround();
+                physics.InputControls.ForwardKeyDown = false;
+                await physics.LookAt(target);
+                physics.InputControls.ForwardKeyDown = true;
+            }
+
+            // keep jumping if below target block
+            physics.InputControls.JumpingKeyDown = player.Entity.Position.Y - target.Y < 0.0;
+
+            var dst = player.Self!.Entity!.Position
+                .DistanceToSquared(target);
+
+            if (dst > prevDst)
+                await physics.LookAt(target);
+
+            if (dst <= IMove.THRESHOLD_COMPLETED)
+            {
+                break;
+            }
+        }
+
+        physics.InputControls.Reset();
+    }
+}