3 files changed, 20 insertions, 17 deletions

diff --git a/src/Entities/Entity.cpp b/src/Entities/Entity.cpp
index dff7114d5..f776f881a 100644
--- a/src/Entities/Entity.cpp
+++ b/src/Entities/Entity.cpp
@@ -1122,12 +1122,14 @@ void cEntity::HandlePhysics(std::chrono::milliseconds a_Dt, cChunk & a_Chunk)
 		Vector3d HitCoords;
 		Vector3i HitBlockCoords;
 		eBlockFace HitBlockFace;
-		if (cLineBlockTracer::FirstSolidHitTrace(*GetWorld(), NextPos, NextPos + NextSpeed, HitCoords, HitBlockCoords, HitBlockFace))
+		Vector3d wantNextPos = NextPos + NextSpeed * DtSec.count();
+		auto isHit = cLineBlockTracer::FirstSolidHitTrace(*GetWorld(), NextPos, wantNextPos, HitCoords, HitBlockCoords, HitBlockFace);
+		if (isHit)
 		{
 			// Set our position to where the block was hit, minus a bit:
 			// TODO: The real entity's m_Width should be taken into account here
 			NextPos = HitCoords - NextSpeed.NormalizeCopy() * 0.1;
-			if (HitBlockFace == BLOCK_FACE_YM)
+			if (HitBlockFace == BLOCK_FACE_YP)
 			{
 				// We hit the ground, adjust the position to the top of the block:
 				m_bOnGround = true;
@@ -1161,11 +1163,11 @@ void cEntity::HandlePhysics(std::chrono::milliseconds a_Dt, cChunk & a_Chunk)
 				}
 			}
 		}
-	}
-	else
-	{
-		// We didn't hit anything, so move =]
-		NextPos += (NextSpeed * DtSec.count());
+		else
+		{
+			// We didn't hit anything, so move:
+			NextPos += (NextSpeed * DtSec.count());
+		}
 	}
 
 	SetPosition(NextPos);
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. */