3 files changed, 79 insertions, 34 deletions

diff --git a/src/Mobs/Monster.cpp b/src/Mobs/Monster.cpp
index 37774b08f..df9c1f390 100644
--- a/src/Mobs/Monster.cpp
+++ b/src/Mobs/Monster.cpp
@@ -156,6 +156,11 @@ bool cMonster::TickPathFinding(cChunk & a_Chunk)
 
 	if (m_Path == nullptr)
 	{
+		if (!EnsureProperDestination(a_Chunk))
+		{
+			StopMovingToPosition();  // Invalid chunks, probably world is loading or something, cancel movement.
+			return false;
+		}
 		m_PathFinderDestination = m_FinalDestination;
 		m_Path = new cPath(a_Chunk, GetPosition().Floor(), m_PathFinderDestination.Floor(), 20);
 	}
@@ -199,10 +204,12 @@ void cMonster::MoveToWayPoint(cChunk & a_Chunk)
 {
 	if (m_JumpCoolDown == 0)
 	{
-		// We're not moving (or barely moving), and waypoint is above us, it means we are hitting something and we should jump.
-		if ((GetSpeedX() < 0.1) && (GetSpeedZ() < 0.1) && DoesPosYRequireJump(FloorC(m_NextWayPointPosition.y)))
+		if (DoesPosYRequireJump(FloorC(m_NextWayPointPosition.y)))
 		{
-			if (IsOnGround() || IsSwimming())
+			if (
+				(IsOnGround() && (GetSpeedX() == 0) && (GetSpeedY() == 0)) ||
+				(IsSwimming() && (m_GiveUpCounter < 15))
+			)
 			{
 				m_bOnGround = false;
 				m_JumpCoolDown = 20;
@@ -252,6 +259,63 @@ void cMonster::MoveToWayPoint(cChunk & a_Chunk)
 
 
 
+bool cMonster::EnsureProperDestination(cChunk & a_Chunk)
+{
+	cChunk * Chunk = a_Chunk.GetNeighborChunk(m_FinalDestination.x, m_FinalDestination.z);
+	BLOCKTYPE BlockType;
+	NIBBLETYPE BlockMeta;
+	int RelX = m_FinalDestination.x - Chunk->GetPosX() * cChunkDef::Width;
+	int RelZ = m_FinalDestination.z - Chunk->GetPosZ() * cChunkDef::Width;
+	if ((Chunk == nullptr) || !Chunk->IsValid())
+	{
+		return false;
+	}
+
+	// If destination in the air, go down to the lowest air block.
+	while (m_FinalDestination.y > 0)
+	{
+		Chunk->GetBlockTypeMeta(RelX, m_FinalDestination.y - 1, RelZ, BlockType, BlockMeta);
+		if (cBlockInfo::IsSolid(BlockType))
+		{
+			break;
+		}
+		m_FinalDestination.y -= 1;
+	}
+
+
+	// If destination in water, go up to the highest water block.
+	// If destination in solid, go up to first air block.
+	bool InWater = false;
+	while (m_FinalDestination.y < cChunkDef::Height)
+	{
+		Chunk->GetBlockTypeMeta(RelX, m_FinalDestination.y, RelZ, BlockType, BlockMeta);
+		if (BlockType == E_BLOCK_STATIONARY_WATER)
+		{
+			InWater = true;
+		}
+		else if (cBlockInfo::IsSolid(BlockType))
+		{
+			InWater = false;
+		}
+		else
+		{
+			break;
+		}
+		m_FinalDestination.y += 1;
+	}
+	if (InWater)
+	{
+		m_FinalDestination.y -= 1;
+	}
+
+
+	return true;
+}
+
+
+
+
+
 void cMonster::MoveToPosition(const Vector3d & a_Position)
 {
 		m_FinalDestination = a_Position;
@@ -292,7 +356,7 @@ void cMonster::Tick(std::chrono::milliseconds a_Dt, cChunk & a_Chunk)
 
 	if (m_Health <= 0)
 	{
-		// The mob is dead, but we're still animating the "puff" they leave when they die
+		// The mob is dead, but we're still animating the "puff" they leave when they die.
 		m_DestroyTimer += a_Dt;
 		if (m_DestroyTimer > std::chrono::seconds(1))
 		{
@@ -310,7 +374,7 @@ void cMonster::Tick(std::chrono::milliseconds a_Dt, cChunk & a_Chunk)
 		m_Target = nullptr;
 	}
 
-	// Process the undead burning in daylight
+	// Process the undead burning in daylight.
 	HandleDaylightBurning(*Chunk, WouldBurnAt(GetPosition(), *Chunk));
 	if (TickPathFinding(*Chunk))
 	{
@@ -1121,7 +1185,3 @@ cMonster::eFamily cMonster::GetMobFamily(void) const
 {
 	return FamilyFromType(m_MobType);
 }
-
-
-
-
diff --git a/src/Mobs/Monster.h b/src/Mobs/Monster.h
index c7f38c9f7..a2295777a 100644
--- a/src/Mobs/Monster.h
+++ b/src/Mobs/Monster.h
@@ -203,8 +203,18 @@ protected:
 	Returns if a path is ready, and therefore if the mob should move to m_NextWayPointPosition
 	*/
 	bool TickPathFinding(cChunk & a_Chunk);
+
+	/** Move in a straight line to the next waypoint in the path, will jump if needed. */
 	void MoveToWayPoint(cChunk & a_Chunk);
 
+	/** Ensures the destination is not buried underground or under water. Also ensures the destination is not in the air.
+	Only the Y coordinate of m_FinalDestination might be changed.
+	1. If m_FinalDestination is the position of a water block, m_FinalDestination's Y will be modified to point to the heighest water block in the pool in the current column.
+	2. If m_FinalDestination is the position of a solid, m_FinalDestination's Y will be modified to point to the first airblock above the solid in the current column.
+	3. If m_FinalDestination is the position of an air block, Y will keep decreasing until hitting either a solid or water.
+	Now either 1 or 2 is performed. */
+	bool EnsureProperDestination(cChunk & a_Chunk);
+
 	/** Resets a pathfinding task, be it due to failure or something else
 	Resets the pathfinder. If m_IsFollowingPath is true, TickPathFinding starts a brand new path.
 	Should only be called by the pathfinder, cMonster::Tick or StopMovingToPosition. */
diff --git a/src/Mobs/Path.cpp b/src/Mobs/Path.cpp
index 84d888bf2..ebfb5b630 100644
--- a/src/Mobs/Path.cpp
+++ b/src/Mobs/Path.cpp
@@ -54,31 +54,6 @@ cPath::cPath(
 		return;
 	}
 
-	// If destination in water, set water surface as destination.
-	cChunk * Chunk = m_Chunk->GetNeighborChunk(m_Destination.x, m_Destination.z);
-	if ((Chunk != nullptr) && Chunk->IsValid())
-	{
-		BLOCKTYPE BlockType;
-		NIBBLETYPE BlockMeta;
-		int RelX = m_Destination.x - Chunk->GetPosX() * cChunkDef::Width;
-		int RelZ = m_Destination.z - Chunk->GetPosZ() * cChunkDef::Width;
-		bool inwater = false;
-		for (;;)
-		{
-			Chunk->GetBlockTypeMeta(RelX, m_Destination.y, RelZ, BlockType, BlockMeta);
-			if (BlockType != E_BLOCK_STATIONARY_WATER)
-			{
-				break;
-			}
-			inwater = true;
-			m_Destination+=Vector3d(0, 1, 0);
-		}
-		if (inwater)
-		{
-			m_Destination+=Vector3d(0, -1, 0);
-		}
-	}
-
 	m_Status = ePathFinderStatus::CALCULATING;
 	m_StepsLeft = a_MaxSteps;