Implement more realisic fall to ground

Should help moving around in servers using anti-cheat plugins.

See #195

MinecraftClient/Mapping/Movement.cs

@@ -17,15 +17,25 @@ namespace MinecraftClient.Mapping
         /// </summary>
         /// <param name="world">World the player is currently located in</param>
         /// <param name="location">Location the player is currently at</param>
+        /// <param name="motionY">Current vertical motion speed</param>
         /// <returns>Updated location after applying gravity</returns>
-        public static Location HandleGravity(World world, Location location)
+        public static Location HandleGravity(World world, Location location, ref double motionY)
         {
             Location onFoots = new Location(location.X, Math.Floor(location.Y), location.Z);
             Location belowFoots = Move(location, Direction.Down);
+            if (location.Y > Math.Truncate(location.Y) + 0.0001)
+            {
+                belowFoots = location;
+                belowFoots.Y = Math.Truncate(location.Y);
+            }
             if (!IsOnGround(world, location) && !IsSwimming(world, location))
-                location = Move2Steps(location, belowFoots).Dequeue();
+            {
+                while (!IsOnGround(world, belowFoots) && belowFoots.Y >= 1)
+                    belowFoots = Move(belowFoots, Direction.Down);
+                location = Move2Steps(location, belowFoots, ref motionY, true).Dequeue();
+            }
             else if (!(world.GetBlock(onFoots).Type.IsSolid()))
-                location = Move2Steps(location, onFoots).Dequeue();
+                location = Move2Steps(location, onFoots, ref motionY, true).Dequeue();
             return location;
         }
 
@@ -64,25 +74,46 @@ namespace MinecraftClient.Mapping
         /// </remarks>
         /// <param name="start">Start location</param>
         /// <param name="goal">Destination location</param>
+        /// <param name="motionY">Current vertical motion speed</param>
+        /// <param name="falling">Specify if performing falling steps</param>
         /// <param name="stepsByBlock">Amount of steps by block</param>
         /// <returns>A list of locations corresponding to the requested steps</returns>
-        public static Queue<Location> Move2Steps(Location start, Location goal, int stepsByBlock = 8)
+        public static Queue<Location> Move2Steps(Location start, Location goal, ref double motionY, bool falling = false, int stepsByBlock = 8)
         {
             if (stepsByBlock <= 0)
                 stepsByBlock = 1;
 
-            double totalStepsDouble = start.Distance(goal) * stepsByBlock;
+            if (falling)
-            int totalSteps = (int)Math.Ceiling(totalStepsDouble);
-            Location step = (goal - start) / totalSteps;
-
-            if (totalStepsDouble >= 1)
             {
-                Queue<Location> movementSteps = new Queue<Location>();
+                //Use MC-Like falling algorithm
-                for (int i = 1; i <= totalSteps; i++)
+                double Y = start.Y;
-                    movementSteps.Enqueue(start + step * i);
+                Queue<Location> fallSteps = new Queue<Location>();
-                return movementSteps;
+                fallSteps.Enqueue(start);
+                double motionPrev = motionY;
+                motionY -= 0.08D;
+                motionY *= 0.9800000190734863D;
+                Y += motionY;
+                if (Y < goal.Y)
+                    return new Queue<Location>(new[] { goal });
+                else return new Queue<Location>(new[] { new Location(start.X, Y, start.Z) });
+            }
+            else
+            {
+                //Regular MCC moving algorithm
+                motionY = 0; //Reset motion speed
+                double totalStepsDouble = start.Distance(goal) * stepsByBlock;
+                int totalSteps = (int)Math.Ceiling(totalStepsDouble);
+                Location step = (goal - start) / totalSteps;
+
+                if (totalStepsDouble >= 1)
+                {
+                    Queue<Location> movementSteps = new Queue<Location>();
+                    for (int i = 1; i <= totalSteps; i++)
+                        movementSteps.Enqueue(start + step * i);
+                    return movementSteps;
+                }
+                else return new Queue<Location>(new[] { goal });
             }
-            else return new Queue<Location>(new[] { goal });
         }
 
         /// <summary>
@@ -163,7 +194,8 @@ namespace MinecraftClient.Mapping
         /// <returns>True if the specified location is on the ground</returns>
         public static bool IsOnGround(World world, Location location)
         {
-            return world.GetBlock(Move(location, Direction.Down)).Type.IsSolid();
+            return world.GetBlock(Move(location, Direction.Down)).Type.IsSolid()
+                && (location.Y <= Math.Truncate(location.Y) + 0.0001);
         }
 
         /// <summary>

MinecraftClient/McTcpClient.cs

@@ -37,6 +37,8 @@ namespace MinecraftClient
         private Queue<Location> steps;
         private Queue<Location> path;
         private Location location;
+        private byte[] yawpitch;
+        private double motionY;
 
         private string host;
         private int port;
@@ -410,8 +412,9 @@ namespace MinecraftClient
         /// </summary>
         /// <param name="location">The new location</param>
         /// <param name="relative">If true, the location is relative to the current location</param>
-        public void UpdateLocation(Location location)
+        public void UpdateLocation(Location location, byte[] yawpitch)
         {
+            this.yawpitch = yawpitch;
             UpdateLocation(location, false);
         }
 
@@ -520,13 +523,17 @@ namespace MinecraftClient
                 {
                     for (int i = 0; i < 2; i++) //Needs to run at 20 tps; MCC runs at 10 tps
                     {
-                        if (steps != null && steps.Count > 0)
+                        if (yawpitch == null)
-                            location = steps.Dequeue();
+                        {
-                        else if (path != null && path.Count > 0)
+                            if (steps != null && steps.Count > 0)
-                            steps = Movement.Move2Steps(location, path.Dequeue());
+                                location = steps.Dequeue();
-                        else location = Movement.HandleGravity(world, location);
+                            else if (path != null && path.Count > 0)
-                        handler.SendLocationUpdate(location, Movement.IsOnGround(world, location));
+                                steps = Movement.Move2Steps(location, path.Dequeue(), ref motionY);
+                            else location = Movement.HandleGravity(world, location, ref motionY);
+                        }
+                        handler.SendLocationUpdate(location, Movement.IsOnGround(world, location), yawpitch);
                     }
+                    yawpitch = null; //First 2 updates must be player position AND look, and player must not move (to conform with vanilla)
                 }
             }
         }

MinecraftClient/Protocol/Handlers/Protocol16.cs

@@ -637,7 +637,7 @@ namespace MinecraftClient.Protocol.Handlers
             return false; //Currently not implemented
         }
 
-        public bool SendLocationUpdate(Location location, bool onGround)
+        public bool SendLocationUpdate(Location location, bool onGround, byte[] yawpitch)
         {
             return false; //Currently not implemented
         }

MinecraftClient/Protocol/Handlers/Protocol18.cs

@@ -273,7 +273,7 @@ namespace MinecraftClient.Protocol.Handlers
                         double x = readNextDouble(packetData);
                         double y = readNextDouble(packetData);
                         double z = readNextDouble(packetData);
-                        readData(8, packetData); //Ignore look
+                        byte[] yawpitch = readData(8, packetData);
                         byte locMask = readNextByte(packetData);
 
                         if (protocolversion >= MC18Version)
@@ -282,9 +282,9 @@ namespace MinecraftClient.Protocol.Handlers
                             location.X = (locMask & 1 << 0) != 0 ? location.X + x : x;
                             location.Y = (locMask & 1 << 1) != 0 ? location.Y + y : y;
                             location.Z = (locMask & 1 << 2) != 0 ? location.Z + z : z;
-                            handler.UpdateLocation(location);
+                            handler.UpdateLocation(location, yawpitch);
                         }
-                        else handler.UpdateLocation(new Location(x, y, z));
+                        else handler.UpdateLocation(new Location(x, y, z), yawpitch);
 
                         if (protocolversion >= MC19Version)
                         {
@@ -1513,20 +1513,33 @@ namespace MinecraftClient.Protocol.Handlers
         /// </summary>
         /// <param name="location">The new location of the player</param>
         /// <param name="onGround">True if the player is on the ground</param>
+        /// <param name="yawpitch">Yaw and pitch (optional and currently not parsed)</param>
         /// <returns>True if the location update was successfully sent</returns>
-        public bool SendLocationUpdate(Location location, bool onGround)
+        public bool SendLocationUpdate(Location location, bool onGround, byte[] yawpitch = null)
         {
             if (Settings.TerrainAndMovements)
             {
+                int packetId;
+                if (yawpitch != null && yawpitch.Length == 8)
+                {
+                    packetId = protocolversion >= MC19Version ? 0x0D : 0x06;
+                }
+                else
+                {
+                    yawpitch = new byte[0];
+                    packetId = protocolversion >= MC19Version ? 0x0C : 0x04;
+                }
+
                 try
                 {
-                    SendPacket(protocolversion >= MC19Version ? 0x0C : 0x04, concatBytes(
+                    SendPacket(packetId, concatBytes(
                         getDouble(location.X),
                         getDouble(location.Y),
                         protocolversion < MC18Version
                             ? getDouble(location.Y + 1.62)
                             : new byte[0],
                         getDouble(location.Z),
+                        yawpitch,
                         new byte[] { onGround ? (byte)1 : (byte)0 }));
                     return true;
                 }

MinecraftClient/Protocol/IMinecraftCom.cs

@@ -71,8 +71,10 @@ namespace MinecraftClient.Protocol
         /// Send a location update telling that we moved to that location
         /// </summary>
         /// <param name="location">The new location</param>
+        /// <param name="onGround">True if the player is on the ground</param>
+        /// <param name="yawpitch">Yaw and pitch (optional and currently not parsed)</param>
         /// <returns>True if packet was successfully sent</returns>
-        bool SendLocationUpdate(Location location, bool onGround);
+        bool SendLocationUpdate(Location location, bool onGround, byte[] yawpitch);
 
         /// <summary>
         /// Send a plugin channel packet to the server.

MinecraftClient/Protocol/IMinecraftComHandler.cs

@@ -55,7 +55,8 @@ namespace MinecraftClient.Protocol
         /// Called when the server sets the new location for the player
         /// </summary>
         /// <param name="location">New location of the player</param>
-        void UpdateLocation(Location location);
+        /// <param name="yawpitch">Yaw and pitch (optional and currently not parsed)</param>
+        void UpdateLocation(Location location, byte[] yawpitch);
 
         /// <summary>
         /// This method is called when the connection has been lost