diff --git a/MinecraftClient/Commands/Move.cs b/MinecraftClient/Commands/Move.cs
index 8922b88e..1b4f85ee 100644
--- a/MinecraftClient/Commands/Move.cs
+++ b/MinecraftClient/Commands/Move.cs
@@ -82,7 +82,8 @@ namespace MinecraftClient.Commands
                     {
                         if (handler.MoveTo(goal, allowUnsafe: takeRisk))
                             return Translations.Get("cmd.move.moving", args[0]);
-                        else return takeRisk ? Translations.Get("cmd.move.dir_fail") : Translations.Get("cmd.move.suggestforce");
+                        else 
+                            return takeRisk ? Translations.Get("cmd.move.dir_fail") : Translations.Get("cmd.move.suggestforce");
                     }
                     else return Translations.Get("cmd.move.dir_fail");
                 }
@@ -90,7 +91,7 @@ namespace MinecraftClient.Commands
                 {
                     try
                     {
-                        Location current = handler.GetCurrentLocation(), currentCenter = new Location(current).ConvertToCenter();
+                        Location current = handler.GetCurrentLocation(), currentCenter = current.ToCenter();
                         
                         double x = args[0].StartsWith('~') ? current.X + (args[0].Length > 1 ? double.Parse(args[0][1..]) : 0) : double.Parse(args[0]);
                         double y = args[1].StartsWith('~') ? current.Y + (args[1].Length > 1 ? double.Parse(args[1][1..]) : 0) : double.Parse(args[1]);
@@ -103,7 +104,7 @@ namespace MinecraftClient.Commands
 
                         if (takeRisk || Movement.PlayerFitsHere(handler.GetWorld(), goal))
                         {
-                            if (current.DistanceSquared(goal) <= 1.5)
+                            if (current.ToFloor() == goal.ToFloor())
                                 handler.MoveTo(goal, allowDirectTeleport: true);
                             else if (!handler.MoveTo(goal, allowUnsafe: takeRisk))
                                 return takeRisk ? Translations.Get("cmd.move.fail", goal) : Translations.Get("cmd.move.suggestforce", goal);
diff --git a/MinecraftClient/Mapping/Entity.cs b/MinecraftClient/Mapping/Entity.cs
index 82f6a2a9..f2ad04bb 100644
--- a/MinecraftClient/Mapping/Entity.cs
+++ b/MinecraftClient/Mapping/Entity.cs
@@ -141,7 +141,7 @@ namespace MinecraftClient.Mapping
         /// <param name="location">Entity location</param>
         /// <param name="uuid">Player uuid</param>
         /// <param name="name">Player name</param>
-        public Entity(int ID, EntityType type, Location location, Guid uuid, string? name)
+        public Entity(int ID, EntityType type, Location location, Guid uuid, string? name, byte yaw, byte pitch)
         {
             this.ID = ID;
             this.Type = type;
@@ -151,6 +151,8 @@ namespace MinecraftClient.Mapping
             this.Health = 1.0f;
             this.Equipment = new Dictionary<int, Item>();
             this.Item = new Item(ItemType.Air, 0, null);
+            this.Yaw = yaw * (1 / 256) * 360; // to angle in 360 degree
+            this.Pitch = pitch * (1 / 256) * 360;
         }
     }
 }
diff --git a/MinecraftClient/Mapping/Location.cs b/MinecraftClient/Mapping/Location.cs
index d7972b43..edc8404f 100644
--- a/MinecraftClient/Mapping/Location.cs
+++ b/MinecraftClient/Mapping/Location.cs
@@ -76,24 +76,19 @@ namespace MinecraftClient.Mapping
         /// <summary>
         /// Round coordinates
         /// </summary>
-        /// <returns>itself</returns>
-        public Location ConvertToFloor()
+        /// <returns>New location</returns>
+        public Location ToFloor()
         {
-            this.X = Math.Floor(this.X);
-            this.Y = Math.Floor(this.Y);
-            this.Z = Math.Floor(this.Z);
-            return this;
+            return new Location(Math.Floor(this.X), Math.Floor(this.Y), Math.Floor(this.Z));
         }
 
         /// <summary>
         /// Get the center coordinates
         /// </summary>
-        /// <returns>itself</returns>
-        public Location ConvertToCenter()
+        /// <returns>New location</returns>
+        public Location ToCenter()
         {
-            this.X = Math.Floor(this.X) + 0.5;
-            this.Z = Math.Floor(this.Z) + 0.5;
-            return this;
+            return new Location(Math.Floor(this.X) + 0.5, this.Y, Math.Floor(this.Z) + 0.5);
         }
 
         /// <summary>
diff --git a/MinecraftClient/Mapping/Movement.cs b/MinecraftClient/Mapping/Movement.cs
index 5cde8624..8389cdd7 100644
--- a/MinecraftClient/Mapping/Movement.cs
+++ b/MinecraftClient/Mapping/Movement.cs
@@ -49,14 +49,18 @@ namespace MinecraftClient.Mapping
         /// <param name="location">Location the player is currently at</param>
         /// <param name="allowUnsafe">Allow possible but unsafe locations</param>
         /// <returns>A list of new locations the player can move to</returns>
-        public static IEnumerable<Location> GetAvailableMoves(World world, Location location, bool allowUnsafe = false)
+        public static IEnumerable<Location> GetAvailableMoves(World world, Location originLocation, bool allowUnsafe = false)
         {
-            List<Location> availableMoves = new List<Location>();
+            Location location = originLocation.ToCenter();
+            List<Location> availableMoves = new();
             if (IsOnGround(world, location) || IsSwimming(world, location))
             {
                 foreach (Direction dir in Enum.GetValues(typeof(Direction)))
-                    if (CanMove(world, location, dir) && (allowUnsafe || IsSafe(world, Move(location, dir))))
-                        availableMoves.Add(Move(location, dir));
+                {
+                    Location dest = Move(location, dir);
+                    if (CanMove(world, location, dir) && (allowUnsafe || IsSafe(world, dest)))
+                        availableMoves.Add(dest);
+                }
             }
             else
             {
@@ -98,7 +102,8 @@ namespace MinecraftClient.Mapping
                 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 
+                    return new Queue<Location>(new[] { new Location(start.X, Y, start.Z) });
             }
             else
             {
@@ -110,12 +115,13 @@ namespace MinecraftClient.Mapping
 
                 if (totalStepsDouble >= 1)
                 {
-                    Queue<Location> movementSteps = new Queue<Location>();
+                    Queue<Location> movementSteps = new();
                     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 });
             }
         }
 
@@ -168,9 +174,8 @@ namespace MinecraftClient.Mapping
                 throw new ArgumentException("minOffset must be lower or equal to maxOffset", "minOffset");
 
             // Round start coordinates for easier calculation
-            Location startCenter = new Location(start).ConvertToCenter();
-            Location startLower = new Location(start).ConvertToFloor();
-            Location goalLower = new Location(goal).ConvertToFloor();
+            Location startLower = start.ToFloor();
+            Location goalLower = goal.ToFloor();
 
             // We always use distance squared so our limits must also be squared.
             minOffset *= minOffset;
@@ -206,7 +211,7 @@ namespace MinecraftClient.Mapping
                 // Return if goal found and no maxOffset was given OR current node is between minOffset and maxOffset
                 if ((current.Location == goalLower && maxOffset <= 0) || (maxOffset > 0 && current.H_score >= minOffset && current.H_score <= maxOffset))
                 {
-                    return ReconstructPath(CameFrom, current.Location, startCenter, goal);
+                    return ReconstructPath(CameFrom, current.Location, start, goal);
                 }
 
                 // Discover neighbored blocks
@@ -235,7 +240,7 @@ namespace MinecraftClient.Mapping
 
             //// Goal could not be reached. Set the path to the closest location if close enough
             if (current != null && (maxOffset == int.MaxValue || openSet.MinH_ScoreNode.H_score <= maxOffset))
-                return ReconstructPath(CameFrom, openSet.MinH_ScoreNode.Location, startCenter, goal);
+                return ReconstructPath(CameFrom, openSet.MinH_ScoreNode.Location, start, goal);
             else
                 return null;
         }
@@ -246,31 +251,35 @@ namespace MinecraftClient.Mapping
         /// <param name="Came_From">The collection of Locations that leads back to the start</param>
         /// <param name="current">Endpoint of our later walk</param>
         /// <returns>the path that leads to current from the start position</returns>
-        private static Queue<Location> ReconstructPath(Dictionary<Location, Location> Came_From, Location current, Location startCenter, Location end)
+        private static Queue<Location> ReconstructPath(Dictionary<Location, Location> Came_From, Location current, Location start, Location end)
         {
-            // Add 0.5 to walk over the middle of a block and avoid collisions
-            Location center = new(0.5, 0, 0.5);
-
+            int midPathCnt = 0;
             List<Location> total_path = new();
 
             // Move from the center of the block to the final position
-            if (current != end && current.DistanceSquared(end) <= 1.5)
+            if (current != end && current == end.ToFloor())
                 total_path.Add(end);
 
             // Generate intermediate paths
-            total_path.Add(current + center);
+            total_path.Add(current.ToCenter());
             while (Came_From.ContainsKey(current))
             {
+                ++midPathCnt;
                 current = Came_From[current];
-                total_path.Add(current + center);
+                total_path.Add(current.ToCenter());
             }
 
-            // Move to the center of the block first
-            if (current != startCenter && current.DistanceSquared(startCenter) <= 1.5)
-                total_path.Add(startCenter);
+            if (midPathCnt <= 2 && start.DistanceSquared(end) < 2.0)
+                return new Queue<Location>(new Location[] { end });
+            else
+            {
+                // Move to the center of the block first
+                if (current != start && current == start.ToFloor())
+                    total_path.Add(start.ToCenter());
 
-            total_path.Reverse();
-            return new Queue<Location>(total_path);
+                total_path.Reverse();
+                return new Queue<Location>(total_path);
+            }
         }
 
         /// <summary>
@@ -567,7 +576,7 @@ namespace MinecraftClient.Mapping
                     return PlayerFitsHere(world, Move(location, Direction.North)) && PlayerFitsHere(world, Move(location, Direction.West)) && PlayerFitsHere(world, Move(location, direction));
 
                 default:
-                    throw new ArgumentException("Unknown direction", "direction");
+                    throw new ArgumentException("Unknown direction", nameof(direction));
             }
         }
 
diff --git a/MinecraftClient/McClient.cs b/MinecraftClient/McClient.cs
index 62ca784a..3ef7fbf1 100644
--- a/MinecraftClient/McClient.cs
+++ b/MinecraftClient/McClient.cs
@@ -2791,12 +2791,16 @@ namespace MinecraftClient
         /// <summary>
         /// Called when a player spawns or enters the client's render distance
         /// </summary>
-        public void OnSpawnPlayer(int entityID, Guid uuid, Location location, byte Yaw, byte Pitch)
+        public void OnSpawnPlayer(int entityID, Guid uuid, Location location, byte yaw, byte pitch)
         {
-            string? playerName = null;
-            if (onlinePlayers.ContainsKey(uuid))
-                playerName = onlinePlayers[uuid].Name;
-            Entity playerEntity = new(entityID, EntityType.Player, location, uuid, playerName);
+            Entity playerEntity;
+            if (onlinePlayers.TryGetValue(uuid, out PlayerInfo? player))
+            {
+                playerEntity = new(entityID, EntityType.Player, location, uuid, player.Name, yaw, pitch);
+                player.entity = playerEntity;
+            }
+            else
+                playerEntity = new(entityID, EntityType.Player, location, uuid, null, yaw, pitch);
             OnSpawnEntity(playerEntity);
         }
 
diff --git a/MinecraftClient/Protocol/PlayerInfo.cs b/MinecraftClient/Protocol/PlayerInfo.cs
index 73d96018..b16376ce 100644
--- a/MinecraftClient/Protocol/PlayerInfo.cs
+++ b/MinecraftClient/Protocol/PlayerInfo.cs
@@ -23,6 +23,12 @@ namespace MinecraftClient.Protocol
 
         public string? DisplayName;
 
+        // Entity info
+
+        public Mapping.Entity? entity;
+
+        // For message signature
+
         private readonly PublicKey? PublicKey;
 
         private readonly DateTime? KeyExpiresAt;