2 files changed, 11 insertions, 10 deletions

diff --git a/src/Mobs/Monster.cpp b/src/Mobs/Monster.cpp
index ef8794e53..00045fc69 100644
--- a/src/Mobs/Monster.cpp
+++ b/src/Mobs/Monster.cpp
@@ -149,20 +149,21 @@ void cMonster::MoveToWayPoint(cChunk & a_Chunk)
 		return;
 	}
 
-
-	if (m_JumpCoolDown == 0)
+	if (m_JumpCoolDown <= 0)
 	{
 		if (DoesPosYRequireJump(FloorC(m_NextWayPointPosition.y)))
 		{
-			if (((IsOnGround()) && (GetSpeed().SqrLength() == 0.0f)) ||
-			(IsSwimming()))
+			if (
+				(IsOnGround() && (GetSpeed().SqrLength() <= 0.5)) ||  // If walking on the ground, we need to slow down first, otherwise we miss the jump
+				IsSwimming()
+			)
 			{
 				m_bOnGround = false;
 				m_JumpCoolDown = 20;
-				// TODO: Change to AddSpeedY once collision detection is fixed - currently, mobs will go into blocks attempting to jump without a teleport
 				AddPosY(1.6);  // Jump!!
+				SetSpeedY(1);
 				SetSpeedX(3.2 * (m_NextWayPointPosition.x - GetPosition().x));  // Move forward in a preset speed.
-				SetSpeedZ(3.2 * (m_NextWayPointPosition.z - GetPosition().z));  // The numbers were picked based on trial and error and 1.6 and 3.2 are perfect.
+				SetSpeedZ(3.2 * (m_NextWayPointPosition.z - GetPosition().z));  // The numbers were picked based on trial and error
 			}
 		}
 	}
@@ -172,7 +173,7 @@ void cMonster::MoveToWayPoint(cChunk & a_Chunk)
 	}
 
 	Vector3d Distance = m_NextWayPointPosition - GetPosition();
-	if ((Distance.x != 0.0f) || (Distance.z != 0.0f))
+	if ((std::abs(Distance.x) > 0.05) || (std::abs(Distance.z) > 0.05))
 	{
 		Distance.y = 0;
 		Distance.Normalize();
diff --git a/src/Mobs/Monster.h b/src/Mobs/Monster.h
index a4d256578..6dac2c1fb 100644
--- a/src/Mobs/Monster.h
+++ b/src/Mobs/Monster.h
@@ -208,10 +208,10 @@ protected:
 	/** Returns whether or not the target is close enough for attack. */
 	bool TargetIsInRange(void) { ASSERT(m_Target != nullptr); return ((m_Target->GetPosition() - GetPosition()).SqrLength() < (m_AttackRange * m_AttackRange)); }
 
-	/** Returns if a monster can reach a given height by jumping. */
-	inline bool DoesPosYRequireJump(int a_PosY)
+	/** Returns whether the monster needs to jump to reach a given height. */
+	inline bool DoesPosYRequireJump(double a_PosY)
 	{
-		return ((a_PosY > POSY_TOINT));
+		return (a_PosY > GetPosY() + 0.8);  // Assume that differences up to 0.8 blocks can be walked instead of jumped
 	}
 
 	/** Move in a straight line to the next waypoint in the path, will jump if needed. */