14 files changed, 837 insertions, 288 deletions

diff --git a/src/Mobs/AggressiveMonster.cpp b/src/Mobs/AggressiveMonster.cpp
index 526b39e39..d0fb79f6d 100644
--- a/src/Mobs/AggressiveMonster.cpp
+++ b/src/Mobs/AggressiveMonster.cpp
@@ -37,10 +37,7 @@ void cAggressiveMonster::InStateChasing(std::chrono::milliseconds a_Dt)
 			}
 		}
 
-		if (!IsMovingToTargetPosition())
-		{
-			MoveToPosition(m_Target->GetPosition());
-		}
+		MoveToPosition(m_Target->GetPosition());
 	}
 }
 
@@ -100,7 +97,7 @@ void cAggressiveMonster::Attack(std::chrono::milliseconds a_Dt)
 	{
 		return;
 	}
-	
+
 	// Setting this higher gives us more wiggle room for attackrate
 	m_AttackInterval = 0.0;
 	m_Target->TakeDamage(dtMobAttack, this, m_AttackDamage, 0);
diff --git a/src/Mobs/CMakeLists.txt b/src/Mobs/CMakeLists.txt
index 7a291dcf2..ffbcdf3ea 100644
--- a/src/Mobs/CMakeLists.txt
+++ b/src/Mobs/CMakeLists.txt
@@ -24,6 +24,7 @@ SET (SRCS
 	Mooshroom.cpp
 	PassiveAggressiveMonster.cpp
 	PassiveMonster.cpp
+	Path.cpp
 	Pig.cpp
 	Rabbit.cpp
 	Sheep.cpp
@@ -62,6 +63,7 @@ SET (HDRS
 	Ocelot.h
 	PassiveAggressiveMonster.h
 	PassiveMonster.h
+	Path.h
 	Pig.h
 	Rabbit.h
 	Sheep.h
diff --git a/src/Mobs/Monster.cpp b/src/Mobs/Monster.cpp
index 55d83302a..84f58ff85 100644
--- a/src/Mobs/Monster.cpp
+++ b/src/Mobs/Monster.cpp
@@ -13,7 +13,7 @@
 #include "../Chunk.h"
 #include "../FastRandom.h"
 
-
+#include "Path.h"
 
 
 
@@ -74,8 +74,12 @@ cMonster::cMonster(const AString & a_ConfigName, eMonsterType a_MobType, const A
 	, m_EMState(IDLE)
 	, m_EMPersonality(AGGRESSIVE)
 	, m_Target(nullptr)
-	, m_bMovingToDestination(false)
+	, m_Path(nullptr)
+	, m_IsFollowingPath(false)
+	, m_GiveUpCounter(0)
+	, m_TicksSinceLastPathReset(1000)
 	, m_LastGroundHeight(POSY_TOINT)
+	, m_JumpCoolDown(0)
 	, m_IdleInterval(0)
 	, m_DestroyTimer(0)
 	, m_MobType(a_MobType)
@@ -94,8 +98,9 @@ cMonster::cMonster(const AString & a_ConfigName, eMonsterType a_MobType, const A
 	, m_DropChanceLeggings(0.085f)
 	, m_DropChanceBoots(0.085f)
 	, m_CanPickUpLoot(true)
+	, m_TicksSinceLastDamaged(100)
 	, m_BurnsInDaylight(false)
-	, m_RelativeWalkSpeed(1.0)
+	, m_RelativeWalkSpeed(1)
 {
 	if (!a_ConfigName.empty())
 	{
@@ -116,89 +121,137 @@ void cMonster::SpawnOn(cClientHandle & a_Client)
 
 
 
-void cMonster::TickPathFinding()
+bool cMonster::TickPathFinding(cChunk & a_Chunk)
 {
-	const int PosX = POSX_TOINT;
-	const int PosY = POSY_TOINT;
-	const int PosZ = POSZ_TOINT;
-
-	std::vector<Vector3d> m_PotentialCoordinates;
-	m_TraversedCoordinates.push_back(Vector3i(PosX, PosY, PosZ));
-
-	static const struct  // Define which directions to try to move to
+	if (!m_IsFollowingPath)
 	{
-		int x, z;
-	} gCrossCoords[] =
+		return false;
+	}
+	if (m_TicksSinceLastPathReset < 1000)
 	{
-		{ 1, 0},
-		{-1, 0},
-		{ 0, 1},
-		{ 0, -1},
-	} ;
-	
-	if ((PosY - 1 < 0) || (PosY + 2 >= cChunkDef::Height) /* PosY + 1 will never be true if PosY + 2 is not */)
+		// No need to count beyond 1000. 1000 is arbitary here.
+		++m_TicksSinceLastPathReset;
+	}
+
+	if (ReachedFinalDestination())
 	{
-		// Too low/high, can't really do anything
-		FinishPathFinding();
-		return;
+		StopMovingToPosition();
+		return false;
 	}
 
-	for (size_t i = 0; i < ARRAYCOUNT(gCrossCoords); i++)
+	if ((m_FinalDestination - m_PathFinderDestination).Length() > 0.25)  // if the distance between where we're going and where we should go is too big.
 	{
-		if (IsCoordinateInTraversedList(Vector3i(gCrossCoords[i].x + PosX, PosY, gCrossCoords[i].z + PosZ)))
+		/* If we reached the last path waypoint,
+		Or if we haven't re-calculated for too long.
+		Interval is proportional to distance squared, and its minimum is 10.
+		(Recalculate lots when close, calculate rarely when far) */
+		if (
+			((GetPosition() - m_PathFinderDestination).Length() < 0.25) ||
+			((m_TicksSinceLastPathReset > 10) && (m_TicksSinceLastPathReset > (0.15 * (m_FinalDestination - GetPosition()).SqrLength())))
+		)
 		{
-			continue;
+			ResetPathFinding();
 		}
+	}
 
-		BLOCKTYPE BlockAtY = m_World->GetBlock(gCrossCoords[i].x + PosX, PosY, gCrossCoords[i].z + PosZ);
-		BLOCKTYPE BlockAtYP = m_World->GetBlock(gCrossCoords[i].x + PosX, PosY + 1, gCrossCoords[i].z + PosZ);
-		BLOCKTYPE BlockAtYPP = m_World->GetBlock(gCrossCoords[i].x + PosX, PosY + 2, gCrossCoords[i].z + PosZ);
-		int LowestY = FindFirstNonAirBlockPosition(gCrossCoords[i].x + PosX, gCrossCoords[i].z + PosZ);
-		BLOCKTYPE BlockAtLowestY = (LowestY >= cChunkDef::Height) ? E_BLOCK_AIR : m_World->GetBlock(gCrossCoords[i].x + PosX, LowestY, gCrossCoords[i].z + PosZ);
+	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);
+	}
 
-		if (
-			(!cBlockInfo::IsSolid(BlockAtY)) &&
-			(!cBlockInfo::IsSolid(BlockAtYP)) &&
-			(!IsBlockLava(BlockAtLowestY)) &&
-			(BlockAtLowestY != E_BLOCK_CACTUS) &&
-			(PosY - LowestY < FALL_DAMAGE_HEIGHT)
-			)
+	switch (m_Path->Step(a_Chunk))
+	{
+		case ePathFinderStatus::PATH_NOT_FOUND:
 		{
-			m_PotentialCoordinates.push_back(Vector3d((gCrossCoords[i].x + PosX), PosY, gCrossCoords[i].z + PosZ));
+			StopMovingToPosition();  // Give up pathfinding to that destination.
+			break;
 		}
-		else if (
-			(cBlockInfo::IsSolid(BlockAtY)) &&
-			(BlockAtY != E_BLOCK_CACTUS) &&
-			(!cBlockInfo::IsSolid(BlockAtYP)) &&
-			(!cBlockInfo::IsSolid(BlockAtYPP)) &&
-			(BlockAtY != E_BLOCK_FENCE) &&
-			(BlockAtY != E_BLOCK_FENCE_GATE)
-			)
+		case ePathFinderStatus::CALCULATING:
 		{
-			m_PotentialCoordinates.push_back(Vector3d((gCrossCoords[i].x + PosX), PosY + 1, gCrossCoords[i].z + PosZ));
+			// Pathfinder needs more time
+			break;
+		}
+		case ePathFinderStatus::PATH_FOUND:
+		{
+			if (--m_GiveUpCounter == 0)
+			{
+				ResetPathFinding();  // Try to calculate a path again.
+				return false;
+			}
+			else if (!m_Path->IsLastPoint() && (m_Path->IsFirstPoint() || ReachedNextWaypoint()))  // Have we arrived at the next cell, as denoted by m_NextWayPointPosition?
+			{
+				m_NextWayPointPosition = Vector3d(0.5, 0, 0.5) + m_Path->GetNextPoint();
+				m_GiveUpCounter = 40;  // Give up after 40 ticks (2 seconds) if failed to reach m_NextWayPointPosition.
+			}
+			return true;
 		}
 	}
 
-	if (!m_PotentialCoordinates.empty())
+	return false;
+}
+
+
+
+
+
+void cMonster::MoveToWayPoint(cChunk & a_Chunk)
+{
+	if (m_JumpCoolDown == 0)
 	{
-		Vector3f ShortestCoords = m_PotentialCoordinates.front();
-		for (std::vector<Vector3d>::const_iterator itr = m_PotentialCoordinates.begin(); itr != m_PotentialCoordinates.end(); ++itr)
+		if (DoesPosYRequireJump(FloorC(m_NextWayPointPosition.y)))
 		{
-			Vector3f Distance = m_FinalDestination - ShortestCoords;
-			Vector3f Distance2 = m_FinalDestination - *itr;
-			if (Distance.SqrLength() > Distance2.SqrLength())
+			if (
+				(IsOnGround() && (GetSpeedX() == 0) && (GetSpeedY() == 0)) ||
+				(IsSwimming() && (m_GiveUpCounter < 15))
+			)
 			{
-				ShortestCoords = *itr;
+				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!!
+				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.
 			}
 		}
-
-		m_Destination = ShortestCoords;
-		m_Destination.z += 0.5f;
-		m_Destination.x += 0.5f;
 	}
 	else
 	{
-		FinishPathFinding();
+		--m_JumpCoolDown;
+	}
+
+	Vector3d Distance = m_NextWayPointPosition - GetPosition();
+	if ((Distance.x != 0) || (Distance.z != 0))
+	{
+		Distance.y = 0;
+		Distance.Normalize();
+
+		if (m_bOnGround)
+		{
+			Distance *= 2.5f;
+		}
+		else if (IsSwimming())
+		{
+			Distance *= 1.3f;
+		}
+		else
+		{
+			// Don't let the mob move too much if he's falling.
+			Distance *= 0.25f;
+		}
+		// Apply walk speed:
+		Distance *= m_RelativeWalkSpeed;
+		/* Reduced default speed.
+		Close to Vanilla, easier for mobs to follow m_NextWayPointPositions, hence
+		better pathfinding. */
+		Distance *= 0.5;
+		AddSpeedX(Distance.x);
+		AddSpeedZ(Distance.z);
 	}
 }
 
@@ -206,47 +259,90 @@ void cMonster::TickPathFinding()
 
 
 
-void cMonster::MoveToPosition(const Vector3d & a_Position)
+bool cMonster::EnsureProperDestination(cChunk & a_Chunk)
 {
-	FinishPathFinding();
+	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;
+	}
 
-	m_FinalDestination = a_Position;
-	m_bMovingToDestination = true;
-	TickPathFinding();
+
+	return true;
 }
 
 
 
-bool cMonster::IsCoordinateInTraversedList(Vector3i a_Coords)
+
+
+void cMonster::MoveToPosition(const Vector3d & a_Position)
 {
-	return (std::find(m_TraversedCoordinates.begin(), m_TraversedCoordinates.end(), a_Coords) != m_TraversedCoordinates.end());
+		m_FinalDestination = a_Position;
+		m_IsFollowingPath = true;
 }
 
 
 
 
 
-bool cMonster::ReachedDestination()
+void cMonster::StopMovingToPosition()
 {
-	if ((m_Destination - GetPosition()).Length() < 0.5f)
-	{
-		return true;
-	}
-
-	return false;
+	m_IsFollowingPath = false;
 }
 
 
 
 
-bool cMonster::ReachedFinalDestination()
+
+void cMonster::ResetPathFinding(void)
 {
-	if ((GetPosition() - m_FinalDestination).Length() <= m_AttackRange)
+	m_TicksSinceLastPathReset = 0;
+	if (m_Path != nullptr)
 	{
-		return true;
+		delete m_Path;
+		m_Path = nullptr;
 	}
-	
-	return false;
 }
 
 
@@ -256,10 +352,11 @@ bool cMonster::ReachedFinalDestination()
 void cMonster::Tick(std::chrono::milliseconds a_Dt, cChunk & a_Chunk)
 {
 	super::Tick(a_Dt, a_Chunk);
+	GET_AND_VERIFY_CURRENT_CHUNK(Chunk, POSX_TOINT, POSZ_TOINT);
 
 	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))
 		{
@@ -268,73 +365,36 @@ void cMonster::Tick(std::chrono::milliseconds a_Dt, cChunk & a_Chunk)
 		return;
 	}
 
+	if (m_TicksSinceLastDamaged < 100)
+	{
+		++m_TicksSinceLastDamaged;
+	}
 	if ((m_Target != nullptr) && m_Target->IsDestroyed())
 	{
 		m_Target = nullptr;
 	}
 
-	// Burning in daylight
-	HandleDaylightBurning(a_Chunk);
-
-	if (m_bMovingToDestination)
+	// Process the undead burning in daylight.
+	HandleDaylightBurning(*Chunk, WouldBurnAt(GetPosition(), *Chunk));
+	if (TickPathFinding(*Chunk))
 	{
-		if (m_bOnGround)
-		{
-			if (DoesPosYRequireJump((int)floor(m_Destination.y)))
-			{
-				m_bOnGround = false;
-
-				// TODO: Change to AddSpeedY once collision detection is fixed - currently, mobs will go into blocks attempting to jump without a teleport
-				AddPosY(1.2);  // Jump!!
-			}
-		}
-
-		Vector3d Distance = m_Destination - GetPosition();
-		if (!ReachedDestination() && !ReachedFinalDestination())  // If we haven't reached any sort of destination, move
+		/* If I burn in daylight, and I won't burn where I'm standing, and I'll burn in my next position, and at least one of those is true:
+		1. I am idle
+		2. I was not hurt by a player recently.
+		Then STOP. */
+		if (
+			m_BurnsInDaylight && ((m_TicksSinceLastDamaged >= 100) || (m_EMState == IDLE)) &&
+			WouldBurnAt(m_NextWayPointPosition, *Chunk) &&
+			!WouldBurnAt(GetPosition(), *Chunk)
+		)
 		{
-			Distance.y = 0;
-			Distance.Normalize();
-
-			if (m_bOnGround)
-			{
-				Distance *= 2.5f;
-			}
-			else if (IsSwimming())
-			{
-				Distance *= 1.3f;
-			}
-			else
-			{
-				// Don't let the mob move too much if he's falling.
-				Distance *= 0.25f;
-			}
-
-			// Apply walk speed:
-			Distance *= m_RelativeWalkSpeed;
-
-			AddSpeedX(Distance.x);
-			AddSpeedZ(Distance.z);
-
-			// It's too buggy!
-			/*
-			if (m_EMState == ESCAPING)
-			{
-				// Runs Faster when escaping :D otherwise they just walk away
-				SetSpeedX (GetSpeedX() * 2.f);
-				SetSpeedZ (GetSpeedZ() * 2.f);
-			}
-			*/
+			// If we burn in daylight, and we would burn at the next step, and we won't burn where we are right now, and we weren't provoked recently:
+			StopMovingToPosition();
+			m_GiveUpCounter = 40;  // This doesn't count as giving up, keep the giveup timer as is.
 		}
 		else
 		{
-			if (ReachedFinalDestination())  // If we have reached the ultimate, final destination, stop pathfinding and attack if appropriate
-			{
-				FinishPathFinding();
-			}
-			else
-			{
-				TickPathFinding();  // We have reached the next point in our path, calculate another point
-			}
+			MoveToWayPoint(*Chunk);
 		}
 	}
 
@@ -345,13 +405,13 @@ void cMonster::Tick(std::chrono::milliseconds a_Dt, cChunk & a_Chunk)
 	{
 		case IDLE:
 		{
-			// If enemy passive we ignore checks for player visibility
+			// If enemy passive we ignore checks for player visibility.
 			InStateIdle(a_Dt);
 			break;
 		}
 		case CHASING:
 		{
-			// If we do not see a player anymore skip chasing action
+			// If we do not see a player anymore skip chasing action.
 			InStateChasing(a_Dt);
 			break;
 		}
@@ -360,7 +420,6 @@ void cMonster::Tick(std::chrono::milliseconds a_Dt, cChunk & a_Chunk)
 			InStateEscaping(a_Dt);
 			break;
 		}
-			
 		case ATTACKING: break;
 	}  // switch (m_EMState)
 
@@ -370,6 +429,7 @@ void cMonster::Tick(std::chrono::milliseconds a_Dt, cChunk & a_Chunk)
 
 
 
+
 void cMonster::SetPitchAndYawFromDestination()
 {
 	Vector3d FinalDestination = m_FinalDestination;
@@ -377,38 +437,36 @@ void cMonster::SetPitchAndYawFromDestination()
 	{
 		if (m_Target->IsPlayer())
 		{
-			FinalDestination.y = ((cPlayer *)m_Target)->GetStance();
+			FinalDestination.y = static_cast<cPlayer *>(m_Target)->GetStance() - 1;
 		}
 		else
 		{
-			FinalDestination.y = GetHeight();
+			FinalDestination.y = m_Target->GetPosY() + GetHeight();
 		}
 	}
 
 	Vector3d Distance = FinalDestination - GetPosition();
-	if (Distance.SqrLength() > 0.1f)
 	{
-		{
-			double Rotation, Pitch;
-			Distance.Normalize();
-			VectorToEuler(Distance.x, Distance.y, Distance.z, Rotation, Pitch);
-			SetHeadYaw(Rotation);
-			SetPitch(-Pitch);
-		}
+		double Rotation, Pitch;
+		Distance.Normalize();
+		VectorToEuler(Distance.x, Distance.y, Distance.z, Rotation, Pitch);
+		SetHeadYaw(Rotation);
+		SetPitch(-Pitch);
+	}
 
-		{
-			Vector3d BodyDistance = m_Destination - GetPosition();
-			double Rotation, Pitch;
-			Distance.Normalize();
-			VectorToEuler(BodyDistance.x, BodyDistance.y, BodyDistance.z, Rotation, Pitch);
-			SetYaw(Rotation);
-		}
+	{
+		Vector3d BodyDistance = m_NextWayPointPosition - GetPosition();
+		double Rotation, Pitch;
+		BodyDistance.Normalize();
+		VectorToEuler(BodyDistance.x, BodyDistance.y, BodyDistance.z, Rotation, Pitch);
+		SetYaw(Rotation);
 	}
 }
 
 
 
 
+
 void cMonster::HandleFalling()
 {
 	if (m_bOnGround)
@@ -460,7 +518,6 @@ int cMonster::FindFirstNonAirBlockPosition(double a_PosX, double a_PosZ)
 
 
 
-
 bool cMonster::DoTakeDamage(TakeDamageInfo & a_TDI)
 {
 	if (!super::DoTakeDamage(a_TDI))
@@ -476,6 +533,7 @@ bool cMonster::DoTakeDamage(TakeDamageInfo & a_TDI)
 	if (a_TDI.Attacker != nullptr)
 	{
 		m_Target = a_TDI.Attacker;
+		m_TicksSinceLastDamaged = 0;
 	}
 	return true;
 }
@@ -641,7 +699,7 @@ void cMonster::EventLosePlayer(void)
 
 void cMonster::InStateIdle(std::chrono::milliseconds a_Dt)
 {
-	if (m_bMovingToDestination)
+	if (m_IsFollowingPath)
 	{
 		return;  // Still getting there
 	}
@@ -661,14 +719,8 @@ void cMonster::InStateIdle(std::chrono::milliseconds a_Dt)
 		if ((Dist.SqrLength() > 2)  && (rem >= 3))
 		{
 			Vector3d Destination(GetPosX() + Dist.x, 0, GetPosZ() + Dist.z);
-
-			int NextHeight = FindFirstNonAirBlockPosition(Destination.x, Destination.z);
-
-			if (IsNextYPosReachable(NextHeight))
-			{
-				Destination.y = NextHeight;
-				MoveToPosition(Destination);
-			}
+			Destination.y = FindFirstNonAirBlockPosition(Destination.x, Destination.z);
+			MoveToPosition(Destination);
 		}
 	}
 }
@@ -692,7 +744,7 @@ void cMonster::InStateChasing(std::chrono::milliseconds a_Dt)
 void cMonster::InStateEscaping(std::chrono::milliseconds a_Dt)
 {
 	UNUSED(a_Dt);
-	
+
 	if (m_Target != nullptr)
 	{
 		Vector3d newloc = GetPosition();
@@ -771,7 +823,7 @@ AString cMonster::MobTypeToString(eMonsterType a_MobType)
 			return g_MobTypeNames[i].m_lcName;
 		}
 	}
-	
+
 	// Not found:
 	return "";
 }
@@ -866,7 +918,7 @@ cMonster::eFamily cMonster::FamilyFromType(eMonsterType a_Type)
 		case mtWolf:         return mfHostile;
 		case mtZombie:       return mfHostile;
 		case mtZombiePigman: return mfHostile;
-			
+
 		case mtInvalidType:  break;
 	}
 	ASSERT(!"Unhandled mob type");
@@ -1041,7 +1093,7 @@ void cMonster::AddRandomArmorDropItem(cItems & a_Drops, short a_LootingLevel)
 			a_Drops.push_back(GetEquippedHelmet());
 		}
 	}
-	
+
 	if (r1.randInt() % 200 < ((m_DropChanceChestplate * 200) + (a_LootingLevel * 2)))
 	{
 		if (!GetEquippedChestplate().IsEmpty())
@@ -1049,7 +1101,7 @@ void cMonster::AddRandomArmorDropItem(cItems & a_Drops, short a_LootingLevel)
 			a_Drops.push_back(GetEquippedChestplate());
 		}
 	}
-	
+
 	if (r1.randInt() % 200 < ((m_DropChanceLeggings * 200) + (a_LootingLevel * 2)))
 	{
 		if (!GetEquippedLeggings().IsEmpty())
@@ -1057,7 +1109,7 @@ void cMonster::AddRandomArmorDropItem(cItems & a_Drops, short a_LootingLevel)
 			a_Drops.push_back(GetEquippedLeggings());
 		}
 	}
-	
+
 	if (r1.randInt() % 200 < ((m_DropChanceBoots * 200) + (a_LootingLevel * 2)))
 	{
 		if (!GetEquippedBoots().IsEmpty())
@@ -1087,50 +1139,62 @@ void cMonster::AddRandomWeaponDropItem(cItems & a_Drops, short a_LootingLevel)
 
 
 
-void cMonster::HandleDaylightBurning(cChunk & a_Chunk)
+void cMonster::HandleDaylightBurning(cChunk & a_Chunk, bool WouldBurn)
 {
 	if (!m_BurnsInDaylight)
 	{
 		return;
 	}
-	
+
 	int RelY = POSY_TOINT;
 	if ((RelY < 0) || (RelY >= cChunkDef::Height))
 	{
 		// Outside the world
 		return;
 	}
-	
-	int RelX = POSX_TOINT - GetChunkX() * cChunkDef::Width;
-	int RelZ = POSZ_TOINT - GetChunkZ() * cChunkDef::Width;
-
 	if (!a_Chunk.IsLightValid())
 	{
 		m_World->QueueLightChunk(GetChunkX(), GetChunkZ());
 		return;
 	}
 
+	if (!IsOnFire() && WouldBurn)
+	{
+		// Burn for 100 ticks, then decide again
+		StartBurning(100);
+	}
+}
+
+
+
+
+bool cMonster::WouldBurnAt(Vector3d a_Location, cChunk & a_Chunk)
+{
+	cChunk * Chunk = a_Chunk.GetNeighborChunk(FloorC(m_NextWayPointPosition.x), FloorC(m_NextWayPointPosition.z));
+	if ((Chunk == nullptr) || (!Chunk->IsValid()))
+	{
+		return false;
+	}
+	int RelX = FloorC(a_Location.x) - a_Chunk.GetPosX() * cChunkDef::Width;
+	int RelY = FloorC(a_Location.y);
+	int RelZ = FloorC(a_Location.z) - a_Chunk.GetPosZ() * cChunkDef::Width;
 	if (
 		(a_Chunk.GetSkyLight(RelX, RelY, RelZ) == 15) &&             // In the daylight
 		(a_Chunk.GetBlock(RelX, RelY, RelZ) != E_BLOCK_SOULSAND) &&  // Not on soulsand
 		(GetWorld()->GetTimeOfDay() < (12000 + 1000)) &&             // It is nighttime
-		!IsOnFire() &&                                               // Not already burning
 		GetWorld()->IsWeatherSunnyAt(POSX_TOINT, POSZ_TOINT)         // Not raining
 	)
 	{
-		// Burn for 100 ticks, then decide again
-		StartBurning(100);
+		return true;
 	}
+	return false;
 }
 
 
 
 
+
 cMonster::eFamily cMonster::GetMobFamily(void) const
 {
 	return FamilyFromType(m_MobType);
 }
-
-
-
-
diff --git a/src/Mobs/Monster.h b/src/Mobs/Monster.h
index 21ed0c25a..a2295777a 100644
--- a/src/Mobs/Monster.h
+++ b/src/Mobs/Monster.h
@@ -10,11 +10,12 @@
 
 
 
-
-
 class cClientHandle;
 class cWorld;
 
+// Fwd: cPath
+enum class ePathFinderStatus;
+class cPath;
 
 
 
@@ -60,8 +61,9 @@ public:
 
 	virtual void OnRightClicked(cPlayer & a_Player) override;
 
+	/** Engage pathfinder and tell it to calculate a path to a given position, and move the mobile accordingly
+	Currently, the mob will only start moving to a new position after the position it is currently going to is reached. */
 	virtual void MoveToPosition(const Vector3d & a_Position);  // tolua_export
-	virtual bool ReachedDestination(void);
 
 	// tolua_begin
 	eMonsterType GetMobType(void) const { return m_MobType; }
@@ -158,19 +160,25 @@ public:
 
 protected:
 
-	/* ======= PATHFINDING ======= */
-
 	/** A pointer to the entity this mobile is aiming to reach */
 	cEntity * m_Target;
+	cPath *  m_Path;  // TODO unique ptr
+
+	/** Stores if mobile is currently moving towards the ultimate, final destination */
+	bool m_IsFollowingPath;
+
+	/* If 0, will give up reaching the next m_NextWayPointPosition and will re-compute path. */
+	int m_GiveUpCounter;
+	int m_TicksSinceLastPathReset;
+
 	/** Coordinates of the next position that should be reached */
-	Vector3d m_Destination;
+	Vector3d m_NextWayPointPosition;
+
 	/** Coordinates for the ultimate, final destination. */
 	Vector3d m_FinalDestination;
-	/** Returns if the ultimate, final destination has been reached */
-	bool ReachedFinalDestination(void);
 
-	/** Stores if mobile is currently moving towards the ultimate, final destination */
-	bool m_bMovingToDestination;
+	/** Coordinates for the ultimate, final destination last given to the pathfinder. */
+	Vector3d m_PathFinderDestination;
 
 	/** Finds the lowest non-air block position (not the highest, as cWorld::GetHeight does)
 	If current Y is nonsolid, goes down to try to find a solid block, then returns that + 1
@@ -178,44 +186,50 @@ protected:
 	If no suitable position is found, returns cChunkDef::Height. */
 	int FindFirstNonAirBlockPosition(double a_PosX, double a_PosZ);
 
-	/** Returns if a monster can actually reach a given height by jumping or walking */
-	inline bool IsNextYPosReachable(int a_PosY)
-	{
-		return (
-			(a_PosY <= POSY_TOINT) ||
-			DoesPosYRequireJump(a_PosY)
-			);
-	}
+	/** Returns if the ultimate, final destination has been reached */
+	bool ReachedFinalDestination(void) { return ((m_FinalDestination - GetPosition()).SqrLength() < (m_AttackRange * m_AttackRange)); }
+
+	/** Returns if the intermediate waypoint of m_NextWayPointPosition has been reached */
+	bool ReachedNextWaypoint(void) { return ((m_NextWayPointPosition - GetPosition()).SqrLength() < 0.25); }
+
 	/** Returns if a monster can reach a given height by jumping */
 	inline bool DoesPosYRequireJump(int a_PosY)
 	{
 		return ((a_PosY > POSY_TOINT) && (a_PosY == POSY_TOINT + 1));
 	}
 
-	/** A semi-temporary list to store the traversed coordinates during active pathfinding so we don't visit them again */
-	std::vector<Vector3i> m_TraversedCoordinates;
-	/** Returns if coordinate is in the traversed list */
-	bool IsCoordinateInTraversedList(Vector3i a_Coords);
+	/** Finds the next place to go by calculating a path and setting the m_NextWayPointPosition variable for the next block to head to
+	This is based on the ultimate, final destination and the current position, as well as the A* algorithm, and any environmental hazards
+	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. */
+	void ResetPathFinding(void);
+
+	/** Stops pathfinding
+	Calls ResetPathFinding and sets m_IsFollowingPath to false */
+	void StopMovingToPosition();
 
-	/** Finds the next place to go
-		This is based on the ultimate, final destination and the current position, as well as the traversed coordinates, and any environmental hazards */
-	void TickPathFinding(void);
-	/** Finishes a pathfinding task, be it due to failure or something else */
-	inline void FinishPathFinding(void)
-	{
-		m_TraversedCoordinates.clear();
-		m_bMovingToDestination = false;
-	}
 	/** Sets the body yaw and head yaw/pitch based on next/ultimate destinations */
 	void SetPitchAndYawFromDestination(void);
 
-	/* =========================== */
-	/* ========= FALLING ========= */
-
 	virtual void HandleFalling(void);
 	int m_LastGroundHeight;
-
-	/* =========================== */
+	int m_JumpCoolDown;
 
 	std::chrono::milliseconds m_IdleInterval;
 	std::chrono::milliseconds m_DestroyTimer;
@@ -239,10 +253,11 @@ protected:
 	float m_DropChanceLeggings;
 	float m_DropChanceBoots;
 	bool m_CanPickUpLoot;
+	int m_TicksSinceLastDamaged;  // How many ticks ago we were last damaged by a player?
 
-	void HandleDaylightBurning(cChunk & a_Chunk);
+	void HandleDaylightBurning(cChunk & a_Chunk, bool WouldBurn);
+	bool WouldBurnAt(Vector3d a_Location, cChunk & a_Chunk);
 	bool m_BurnsInDaylight;
-
 	double m_RelativeWalkSpeed;
 
 	/** Adds a random number of a_Item between a_Min and a_Max to itemdrops a_Drops*/
diff --git a/src/Mobs/Path.cpp b/src/Mobs/Path.cpp
new file mode 100644
index 000000000..8abbc4cac
--- /dev/null
+++ b/src/Mobs/Path.cpp
@@ -0,0 +1,365 @@
+
+#include "Globals.h"
+
+#include <cmath>
+
+#include "Path.h"
+#include "../Chunk.h"
+
+#define DISTANCE_MANHATTAN 0  // 1: More speed, a bit less accuracy 0: Max accuracy, less speed.
+#define HEURISTICS_ONLY 0  // 1: Much more speed, much less accurate.
+#define CALCULATIONS_PER_STEP 60  // Higher means more CPU load but faster path calculations.
+// The only version which guarantees the shortest path is 0, 0.
+
+enum class eCellStatus {OPENLIST,  CLOSEDLIST,  NOLIST};
+struct cPathCell
+{
+	Vector3i m_Location;   // Location of the cell in the world.
+	int m_F, m_G, m_H;  // F, G, H as defined in regular A*.
+	eCellStatus m_Status;  // Which list is the cell in? Either non, open, or closed.
+	cPathCell * m_Parent;  // Cell's parent, as defined in regular A*.
+	bool m_IsSolid;	   // Is the cell an air or a solid? Partial solids are currently considered solids.
+};
+
+
+
+
+
+bool compareHeuristics::operator()(cPathCell * & a_Cell1, cPathCell * & a_Cell2)
+{
+	return a_Cell1->m_F > a_Cell2->m_F;
+}
+
+
+
+
+
+/* cPath implementation */
+cPath::cPath(
+	cChunk & a_Chunk,
+	const Vector3i & a_StartingPoint, const Vector3i & a_EndingPoint, int a_MaxSteps,
+	double a_BoundingBoxWidth, double a_BoundingBoxHeight,
+	int a_MaxUp, int a_MaxDown
+) :
+	m_Destination(a_EndingPoint.Floor()),
+	m_Source(a_StartingPoint.Floor()),
+	m_CurrentPoint(0),  // GetNextPoint increments this to 1, but that's fine, since the first cell is always a_StartingPoint
+	m_Chunk(&a_Chunk)
+{
+	// TODO: if src not walkable OR dest not walkable, then abort.
+	// Borrow a new "isWalkable" from ProcessIfWalkable, make ProcessIfWalkable also call isWalkable
+
+	if (GetCell(m_Source)->m_IsSolid || GetCell(m_Destination)->m_IsSolid)
+	{
+		m_Status = ePathFinderStatus::PATH_NOT_FOUND;
+		return;
+	}
+
+	m_Status = ePathFinderStatus::CALCULATING;
+	m_StepsLeft = a_MaxSteps;
+
+	ProcessCell(GetCell(a_StartingPoint), nullptr, 0);
+	m_Chunk = nullptr;
+}
+
+
+
+
+
+cPath::~cPath()
+{
+	if (m_Status == ePathFinderStatus::CALCULATING)
+	{
+		FinishCalculation();
+	}
+}
+
+
+
+
+
+ePathFinderStatus cPath::Step(cChunk & a_Chunk)
+{
+	m_Chunk = &a_Chunk;
+
+	if (m_Status != ePathFinderStatus::CALCULATING)
+	{
+		return m_Status;
+	}
+
+	if (m_StepsLeft == 0)
+	{
+		FinishCalculation(ePathFinderStatus::PATH_NOT_FOUND);
+	}
+	else
+	{
+		--m_StepsLeft;
+		int i;
+		for (i = 0; i < CALCULATIONS_PER_STEP; ++i)
+		{
+			if (Step_Internal())  // Step_Internal returns true when no more calculation is needed.
+			{
+				break;  // if we're here, m_Status must have changed either to PATH_FOUND or PATH_NOT_FOUND.
+			}
+		}
+	}
+
+	m_Chunk = nullptr;
+	return m_Status;
+}
+
+
+
+
+
+bool cPath::IsSolid(const Vector3i & a_Location)
+{
+	ASSERT(m_Chunk != nullptr);
+
+	auto Chunk = m_Chunk->GetNeighborChunk(a_Location.x, a_Location.z);
+	if ((Chunk == nullptr) || !Chunk->IsValid())
+	{
+		return true;
+	}
+	m_Chunk = Chunk;
+
+	BLOCKTYPE BlockType;
+	NIBBLETYPE BlockMeta;
+	int RelX = a_Location.x - m_Chunk->GetPosX() * cChunkDef::Width;
+	int RelZ = a_Location.z - m_Chunk->GetPosZ() * cChunkDef::Width;
+
+	m_Chunk->GetBlockTypeMeta(RelX, a_Location.y, RelZ, BlockType, BlockMeta);
+	if ((BlockType == E_BLOCK_FENCE) || (BlockType == E_BLOCK_FENCE_GATE))
+	{
+		GetCell(a_Location + Vector3i(0, 1, 0))->m_IsSolid = true;  // Mobs will always think that the fence is 2 blocks high and therefore won't jump over.
+	}
+	if (BlockType == E_BLOCK_STATIONARY_WATER)
+	{
+		GetCell(a_Location + Vector3i(0, -1, 0))->m_IsSolid = true;  // Mobs will always think that the fence is 2 blocks high and therefore won't jump over.
+	}
+
+	return cBlockInfo::IsSolid(BlockType);
+}
+
+
+
+
+
+bool cPath::Step_Internal()
+{
+	cPathCell * CurrentCell = OpenListPop();
+
+	// Path not reachable, open list exauhsted.
+	if (CurrentCell == nullptr)
+	{
+		FinishCalculation(ePathFinderStatus::PATH_NOT_FOUND);
+		ASSERT(m_Status == ePathFinderStatus::PATH_NOT_FOUND);
+		return true;
+	}
+
+	// Path found.
+	if (
+			(CurrentCell->m_Location == m_Destination + Vector3i(0, 0, 1)) ||
+			(CurrentCell->m_Location == m_Destination + Vector3i(1, 0, 0)) ||
+			(CurrentCell->m_Location == m_Destination + Vector3i(-1, 0, 0)) ||
+			(CurrentCell->m_Location == m_Destination + Vector3i(0, 0, -1)) ||
+			(CurrentCell->m_Location == m_Destination + Vector3i(0, -1, 0))
+	)
+	{
+		do
+		{
+			m_PathPoints.push_back(CurrentCell->m_Location);  // Populate the cPath with points.
+			CurrentCell = CurrentCell->m_Parent;
+		} while (CurrentCell != nullptr);
+
+		FinishCalculation(ePathFinderStatus::PATH_FOUND);
+		return true;
+	}
+
+	// Calculation not finished yet, process a currentCell by inspecting all neighbors.
+
+	// Check North, South, East, West on all 3 different heights.
+	int i;
+	for (i = -1; i <= 1; ++i)
+	{
+		ProcessIfWalkable(CurrentCell->m_Location + Vector3i(1, i, 0),  CurrentCell, 10);
+		ProcessIfWalkable(CurrentCell->m_Location + Vector3i(-1, i, 0), CurrentCell, 10);
+		ProcessIfWalkable(CurrentCell->m_Location + Vector3i(0, i, 1),  CurrentCell, 10);
+		ProcessIfWalkable(CurrentCell->m_Location + Vector3i(0, i, -1), CurrentCell, 10);
+	}
+
+	// Check diagonals on mob's height only.
+	int x, z;
+	for (x = -1; x <= 1; x += 2)
+	{
+		for (z = -1; z <= 1; z += 2)
+		{
+			// This condition prevents diagonal corner cutting.
+			if (!GetCell(CurrentCell->m_Location + Vector3i(x, 0, 0))->m_IsSolid && !GetCell(CurrentCell->m_Location + Vector3i(0, 0, z))->m_IsSolid)
+			{
+				// This prevents falling of "sharp turns" e.g. a 1x1x20 rectangle in the air which breaks in a right angle suddenly.
+				if (GetCell(CurrentCell->m_Location + Vector3i(x, -1, 0))->m_IsSolid && GetCell(CurrentCell->m_Location + Vector3i(0, -1, z))->m_IsSolid)
+				{
+					ProcessIfWalkable(CurrentCell->m_Location + Vector3i(x, 0, z), CurrentCell, 14);  // 14 is a good enough approximation of sqrt(10 + 10).
+				}
+			}
+		}
+	}
+
+	return false;
+}
+
+
+
+
+
+void cPath::FinishCalculation()
+{
+	for (auto && pair : m_Map)
+	{
+		delete pair.second;
+	}
+
+	m_Map.clear();
+	m_OpenList = std::priority_queue<cPathCell *, std::vector<cPathCell *>, compareHeuristics>{};
+}
+
+
+
+
+
+void cPath::FinishCalculation(ePathFinderStatus a_NewStatus)
+{
+	m_Status = a_NewStatus;
+	FinishCalculation();
+}
+
+
+
+
+
+void cPath::OpenListAdd(cPathCell * a_Cell)
+{
+	a_Cell->m_Status = eCellStatus::OPENLIST;
+	m_OpenList.push(a_Cell);
+	#ifdef COMPILING_PATHFIND_DEBUGGER
+	si::setBlock(a_Cell->m_Location.x, a_Cell->m_Location.y, a_Cell->m_Location.z, debug_open, SetMini(a_Cell));
+	#endif
+}
+
+
+
+
+
+cPathCell * cPath::OpenListPop()  // Popping from the open list also means adding to the closed list.
+{
+	if (m_OpenList.size() == 0)
+	{
+		return nullptr;  // We've exhausted the search space and nothing was found, this will trigger a PATH_NOT_FOUND status.
+	}
+
+	cPathCell * Ret = m_OpenList.top();
+	m_OpenList.pop();
+	Ret->m_Status = eCellStatus::CLOSEDLIST;
+	#ifdef COMPILING_PATHFIND_DEBUGGER
+si::setBlock((Ret)->m_Location.x, (Ret)->m_Location.y, (Ret)->m_Location.z, debug_closed, SetMini(Ret));
+	#endif
+	return Ret;
+}
+
+
+
+
+
+void cPath::ProcessIfWalkable(const Vector3i & a_Location, cPathCell * a_Parent, int a_Cost)
+{
+	cPathCell * cell = GetCell(a_Location);
+	if (!cell->m_IsSolid && GetCell(a_Location + Vector3i(0, -1, 0))->m_IsSolid && !GetCell(a_Location + Vector3i(0, 1, 0))->m_IsSolid)
+	{
+		ProcessCell(cell, a_Parent, a_Cost);
+	}
+}
+
+
+
+
+
+void cPath::ProcessCell(cPathCell * a_Cell, cPathCell * a_Caller, int a_GDelta)
+{
+	// Case 1: Cell is in the closed list, ignore it.
+	if (a_Cell->m_Status == eCellStatus::CLOSEDLIST)
+	{
+		return;
+	}
+	if (a_Cell->m_Status == eCellStatus::NOLIST)  // Case 2: The cell is not in any list.
+	{
+		// Cell is walkable, add it to the open list.
+		// Note that non-walkable cells are filtered out in Step_internal();
+		// Special case: Start cell goes here, gDelta is 0, caller is NULL.
+		a_Cell->m_Parent = a_Caller;
+		if (a_Caller != nullptr)
+		{
+			a_Cell->m_G = a_Caller->m_G + a_GDelta;
+		}
+		else
+		{
+			a_Cell->m_G = 0;
+		}
+
+		// Calculate H. This is A*'s Heuristics value.
+		#if DISTANCE_MANHATTAN == 1
+			// Manhattan distance. DeltaX + DeltaY + DeltaZ.
+			a_Cell->m_H = 10 * (abs(a_Cell->m_Location.x-m_Destination.x) + abs(a_Cell->m_Location.y-m_Destination.y) + abs(a_Cell->m_Location.z-m_Destination.z));
+		#else
+			// Euclidian distance. sqrt(DeltaX^2 + DeltaY^2 + DeltaZ^2), more precise.
+			a_Cell->m_H = static_cast<decltype(a_Cell->m_H)>((a_Cell->m_Location - m_Destination).Length() * 10);
+		#endif
+
+		#if HEURISTICS_ONLY == 1
+			a_Cell->m_F = a_Cell->m_H;  // Greedy search. https://en.wikipedia.org/wiki/Greedy_search
+		#else
+			a_Cell->m_F = a_Cell->m_H + a_Cell->m_G;  // Regular A*.
+		#endif
+
+		OpenListAdd(a_Cell);
+		return;
+	}
+
+	// Case 3: Cell is in the open list, check if G and H need an update.
+	int NewG = a_Caller->m_G + a_GDelta;
+	if (NewG < a_Cell->m_G)
+	{
+		a_Cell->m_G = NewG;
+		a_Cell->m_H = a_Cell->m_F + a_Cell->m_G;
+		a_Cell->m_Parent = a_Caller;
+	}
+
+}
+
+
+
+
+
+cPathCell * cPath::GetCell(const Vector3i & a_Location)
+{
+	// Create the cell in the hash table if it's not already there.
+	cPathCell * Cell;
+	if (m_Map.count(a_Location) == 0)  // Case 1: Cell is not on any list. We've never checked this cell before.
+	{
+		Cell = new cPathCell();
+		Cell->m_Location = a_Location;
+		m_Map[a_Location] = Cell;
+		Cell->m_IsSolid = IsSolid(a_Location);
+		Cell->m_Status = eCellStatus::NOLIST;
+		#ifdef COMPILING_PATHFIND_DEBUGGER
+			#ifdef COMPILING_PATHFIND_DEBUGGER_MARK_UNCHECKED
+				si::setBlock(a_Location.x, a_Location.y, a_Location.z, debug_unchecked, Cell->m_IsSolid ? NORMAL : MINI);
+			#endif
+		#endif
+		return Cell;
+	}
+	else
+	{
+		return m_Map[a_Location];
+	}
+}
diff --git a/src/Mobs/Path.h b/src/Mobs/Path.h
new file mode 100644
index 000000000..0d903adb6
--- /dev/null
+++ b/src/Mobs/Path.h
@@ -0,0 +1,151 @@
+
+#pragma once
+
+/* Wanna use the pathfinder? Put this in your header file:
+
+// Fwd: cPath
+enum class ePathFinderStatus;
+class cPath;
+
+Put this in your .cpp:
+#include "...Path.h"
+*/
+
+#ifdef COMPILING_PATHFIND_DEBUGGER
+	/* Note: the COMPILING_PATHFIND_DEBUGGER flag is used by Native/WiseOldMan95 to debug
+	this class outside of MCServer. This preprocessor flag is never set when compiling MCServer. */
+	#include "PathFinderIrrlicht_Head.h"
+#endif
+
+#include <unordered_map>
+
+//fwd: ../Chunk.h
+class cChunk;
+
+/* Various little structs and classes */
+enum class ePathFinderStatus {CALCULATING,  PATH_FOUND,  PATH_NOT_FOUND};
+struct cPathCell;  // Defined inside Path.cpp
+class compareHeuristics
+{
+public:
+	bool operator()(cPathCell * & a_V1,  cPathCell * &  a_V2);
+};
+
+class cPath
+{
+public:
+	/** Creates a pathfinder instance. A Mob will probably need a single pathfinder instance for its entire life.
+
+	Note that if you have a man-sized mob (1x1x2, zombies, etc), you are advised to call this function without parameters
+	because the declaration might change in later version of the pathFinder, and a parameter-less call always assumes a man-sized mob.
+
+	If your mob is not man-sized, you are advised to use cPath(width, height), this would be compatible with future versions,
+	but please be aware that as of now those parameters will be ignored and your mob will be assumed to be man sized.
+
+	@param a_BoundingBoxWidth the character's boundingbox width in blocks. Currently the parameter is ignored and 1 is assumed.
+	@param a_BoundingBoxHeight the character's boundingbox width in blocks. Currently the parameter is ignored and 2 is assumed.
+	@param a_MaxUp the character's max jump height in blocks. Currently the parameter is ignored and 1 is assumed.
+	@param a_MaxDown How far is the character willing to fall? Currently the parameter is ignored and 1 is assumed. */
+	/** Attempts to find a path starting from source to destination.
+	After calling this, you are expected to call Step() once per tick or once per several ticks until it returns true. You should then call getPath() to obtain the path.
+	Calling this before a path is found resets the current path and starts another search.
+	@param a_StartingPoint The function expects this position to be the lowest block the mob is in, a rule of thumb: "The block where the Zombie's knees are at".
+	@param a_EndingPoint "The block where the Zombie's knees want to be".
+	@param a_MaxSteps The maximum steps before giving up. */
+	cPath(
+		cChunk & a_Chunk,
+		const Vector3i & a_StartingPoint, const Vector3i & a_EndingPoint, int a_MaxSteps,
+		double a_BoundingBoxWidth = 1, double a_BoundingBoxHeight = 2,
+		int a_MaxUp = 1, int a_MaxDown = 1
+	);
+
+	/** Destroys the path and frees its memory. */
+	~cPath();
+
+	/** Performs part of the path calculation and returns true if the path computation has finished. */
+	ePathFinderStatus Step(cChunk & a_Chunk);
+
+	/* Point retrieval functions, inlined for performance. */
+	/** Returns the next point in the path. */
+	inline Vector3i GetNextPoint()
+	{
+		ASSERT(m_Status == ePathFinderStatus::PATH_FOUND);
+		return m_PathPoints[m_PathPoints.size() - 1 - (++m_CurrentPoint)];
+	}
+	/** Checks whether this is the last point or not. Never call getnextPoint when this is true. */
+	inline bool IsLastPoint()
+	{
+		ASSERT(m_Status == ePathFinderStatus::PATH_FOUND);
+		return (m_CurrentPoint == m_PathPoints.size() - 1);
+	}
+	inline bool IsFirstPoint()
+	{
+		ASSERT(m_Status == ePathFinderStatus::PATH_FOUND);
+		return (m_CurrentPoint == 0);
+	}
+	/** Get the point at a_index. Remark: Internally, the indexes are reversed. */
+	inline Vector3i GetPoint(size_t a_index)
+	{
+		ASSERT(m_Status == ePathFinderStatus::PATH_FOUND);
+		ASSERT(a_index < m_PathPoints.size());
+		return m_PathPoints[m_PathPoints.size() - 1 - a_index];
+	}
+	/** Returns the total number of points this path has. */
+	inline int GetPointCount()
+	{
+		ASSERT(m_Status == ePathFinderStatus::PATH_FOUND);
+		return m_PathPoints.size();
+	}
+
+	struct VectorHasher
+	{
+		std::size_t operator()(const Vector3i & a_Vector) const
+		{
+			// Guaranteed to have no hash collisions for any 128x128x128 area. Suitable for pathfinding.
+			int32_t t = 0;
+			t += (int8_t)a_Vector.x;
+			t = t << 8;
+			t += (int8_t)a_Vector.y;
+			t = t << 8;
+			t += (int8_t)a_Vector.z;
+			t = t << 8;
+			return (size_t)t;
+		}
+	};
+private:
+
+	/* General */
+	bool IsSolid(const Vector3i & a_Location);  // Query our hosting world and ask it if there's a solid at a_location.
+	bool Step_Internal();  // The public version just calls this version * CALCULATIONS_PER_CALL times.
+	void FinishCalculation();  // Clears the memory used for calculating the path.
+	void FinishCalculation(ePathFinderStatus a_NewStatus);  // Clears the memory used for calculating the path and changes the status.
+
+	/* Openlist and closedlist management */
+	void OpenListAdd(cPathCell * a_Cell);
+	cPathCell * OpenListPop();
+	void ProcessIfWalkable(const Vector3i &a_Location, cPathCell * a_Parent, int a_Cost);
+
+	/* Map management */
+	void ProcessCell(cPathCell * a_Cell,  cPathCell * a_Caller,  int a_GDelta);
+	cPathCell * GetCell(const Vector3i & a_location);
+
+	/* Pathfinding fields */
+	std::priority_queue<cPathCell *,  std::vector<cPathCell *>,  compareHeuristics> m_OpenList;
+	std::unordered_map<Vector3i,  cPathCell *, VectorHasher> m_Map;
+	Vector3i m_Destination;
+	Vector3i m_Source;
+	int m_StepsLeft;
+
+	/* Control fields */
+	ePathFinderStatus m_Status;
+
+	/* Final path fields */
+	size_t m_CurrentPoint;
+	std::vector<Vector3i> m_PathPoints;
+
+	/* Interfacing with the world */
+	cChunk * m_Chunk;  // Only valid inside Step()!
+	#ifdef COMPILING_PATHFIND_DEBUGGER
+	#include "../path_irrlicht.cpp"
+	#endif
+};
diff --git a/src/Mobs/Pig.cpp b/src/Mobs/Pig.cpp
index edd4d9de4..56d6abfd5 100644
--- a/src/Mobs/Pig.cpp
+++ b/src/Mobs/Pig.cpp
@@ -90,7 +90,6 @@ void cPig::Tick(std::chrono::milliseconds a_Dt, cChunk & a_Chunk)
 		if (m_Attachee->IsPlayer() && (m_Attachee->GetEquippedWeapon().m_ItemType == E_ITEM_CARROT_ON_STICK))
 		{
 			MoveToPosition((m_Attachee->GetPosition()) + (m_Attachee->GetLookVector()*10));
-			m_bMovingToDestination = true;
 		}
 	}
 }
diff --git a/src/Mobs/Sheep.cpp b/src/Mobs/Sheep.cpp
index c0cdec035..ec24f167e 100644
--- a/src/Mobs/Sheep.cpp
+++ b/src/Mobs/Sheep.cpp
@@ -98,7 +98,7 @@ void cSheep::Tick(std::chrono::milliseconds a_Dt, cChunk & a_Chunk)
 
 	if (m_TimeToStopEating > 0)
 	{
-		m_bMovingToDestination = false;  // The sheep should not move when he's eating
+		StopMovingToPosition();
 		m_TimeToStopEating--;
 
 		if (m_TimeToStopEating == 0)
diff --git a/src/Mobs/Skeleton.cpp b/src/Mobs/Skeleton.cpp
index 331c8e8ad..f99404669 100644
--- a/src/Mobs/Skeleton.cpp
+++ b/src/Mobs/Skeleton.cpp
@@ -37,7 +37,7 @@ void cSkeleton::GetDrops(cItems & a_Drops, cEntity * a_Killer)
 	else
 	{
 		AddRandomDropItem(a_Drops, 0, 2 + LootingLevel, E_ITEM_ARROW);
-		
+
 	}
 	AddRandomDropItem(a_Drops, 0, 2 + LootingLevel, E_ITEM_BONE);
 	AddRandomArmorDropItem(a_Drops, LootingLevel);
@@ -48,25 +48,6 @@ void cSkeleton::GetDrops(cItems & a_Drops, cEntity * a_Killer)
 
 
 
-void cSkeleton::MoveToPosition(const Vector3d & a_Position)
-{
-	// If the destination is sufficiently skylight challenged AND the skeleton isn't on fire then block the movement
-	if (
-		!IsOnFire() &&
-		(m_World->GetBlockSkyLight((int)floor(a_Position.x), (int)floor(a_Position.y), (int)floor(a_Position.z)) - m_World->GetSkyDarkness() > 8)
-		)
-	{
-		m_bMovingToDestination = false;
-		return;
-	}
-
-	super::MoveToPosition(a_Position);
-}
-
-
-
-
-
 void cSkeleton::Attack(std::chrono::milliseconds a_Dt)
 {
 	m_AttackInterval += (static_cast<float>(a_Dt.count()) / 1000) * m_AttackRate;
diff --git a/src/Mobs/Skeleton.h b/src/Mobs/Skeleton.h
index 9c49c52fb..1b6ce4bf2 100644
--- a/src/Mobs/Skeleton.h
+++ b/src/Mobs/Skeleton.h
@@ -11,19 +11,18 @@ class cSkeleton :
 	public cAggressiveMonster
 {
 	typedef cAggressiveMonster super;
-	
+
 public:
 	cSkeleton(bool IsWither);
 
 	CLASS_PROTODEF(cSkeleton)
 
 	virtual void GetDrops(cItems & a_Drops, cEntity * a_Killer = nullptr) override;
-	virtual void MoveToPosition(const Vector3d & a_Position) override;
 	virtual void Attack(std::chrono::milliseconds a_Dt) override;
 	virtual void SpawnOn(cClientHandle & a_ClientHandle) override;
 
 	virtual bool IsUndead(void) override { return true; }
-	
+
 	bool IsWither(void) const { return m_bIsWither; }
 
 private:
diff --git a/src/Mobs/Villager.cpp b/src/Mobs/Villager.cpp
index 6f647ac18..e4953d546 100644
--- a/src/Mobs/Villager.cpp
+++ b/src/Mobs/Villager.cpp
@@ -156,7 +156,7 @@ void cVillager::HandleFarmerPrepareFarmCrops()
 void cVillager::HandleFarmerTryHarvestCrops()
 {
 	// Harvest the crops if the villager isn't moving and if the crops are closer then 2 blocks.
-	if (!m_bMovingToDestination && (GetPosition() - m_CropsPos).Length() < 2)
+	if (!m_IsFollowingPath && (GetPosition() - m_CropsPos).Length() < 2)
 	{
 		// Check if the blocks didn't change while the villager was walking to the coordinates.
 		BLOCKTYPE CropBlock = m_World->GetBlock(m_CropsPos.x, m_CropsPos.y, m_CropsPos.z);
diff --git a/src/Mobs/Wolf.cpp b/src/Mobs/Wolf.cpp
index b3eefdf79..3c2ec1520 100644
--- a/src/Mobs/Wolf.cpp
+++ b/src/Mobs/Wolf.cpp
@@ -5,6 +5,7 @@
 #include "../World.h"
 #include "../Entities/Player.h"
 #include "../Items/ItemHandler.h"
+#include "Broadcaster.h"
 
 
 
@@ -83,13 +84,13 @@ void cWolf::OnRightClicked(cPlayer & a_Player)
 				SetIsTame(true);
 				SetOwner(a_Player.GetName(), a_Player.GetUUID());
 				m_World->BroadcastEntityStatus(*this, esWolfTamed);
-				m_World->BroadcastParticleEffect("heart", (float) GetPosX(), (float) GetPosY(), (float) GetPosZ(), 0, 0, 0, 0, 5);
+				m_World->GetBroadcaster().BroadcastParticleEffect("heart", static_cast<Vector3f>(GetPosition()), Vector3f{}, 0, 5);
 			}
 			else
 			{
 				// Taming failed
 				m_World->BroadcastEntityStatus(*this, esWolfTaming);
-				m_World->BroadcastParticleEffect("smoke", (float) GetPosX(), (float) GetPosY(), (float) GetPosZ(), 0, 0, 0, 0, 5);
+				m_World->GetBroadcaster().BroadcastParticleEffect("smoke", static_cast<Vector3f>(GetPosition()), Vector3f{}, 0, 5);
 			}
 		}
 	}
@@ -137,7 +138,7 @@ void cWolf::OnRightClicked(cPlayer & a_Player)
 			}
 		}
 	}
-	
+
 	m_World->BroadcastEntityMetadata(*this);
 }
 
@@ -203,7 +204,7 @@ void cWolf::Tick(std::chrono::milliseconds a_Dt, cChunk & a_Chunk)
 	}
 	else if (IsSitting())
 	{
-		m_bMovingToDestination = false;
+		StopMovingToPosition();
 	}
 }
 
diff --git a/src/Mobs/Zombie.cpp b/src/Mobs/Zombie.cpp
index 63042e252..fa4ac855d 100644
--- a/src/Mobs/Zombie.cpp
+++ b/src/Mobs/Zombie.cpp
@@ -37,26 +37,3 @@ void cZombie::GetDrops(cItems & a_Drops, cEntity * a_Killer)
 	AddRandomArmorDropItem(a_Drops, LootingLevel);
 	AddRandomWeaponDropItem(a_Drops, LootingLevel);
 }
-
-
-
-
-
-void cZombie::MoveToPosition(const Vector3d & a_Position)
-{
-	// If the destination is sufficiently skylight challenged AND the skeleton isn't on fire then block the movement
-	if (
-		!IsOnFire() &&
-		(m_World->GetBlockSkyLight((int)floor(a_Position.x), (int)floor(a_Position.y), (int)floor(a_Position.z)) - m_World->GetSkyDarkness() > 8)
-		)
-	{
-		m_bMovingToDestination = false;
-		return;
-	}
-
-	super::MoveToPosition(a_Position);
-}
-
-
-
-
diff --git a/src/Mobs/Zombie.h b/src/Mobs/Zombie.h
index 809c2a6fe..47a9f1904 100644
--- a/src/Mobs/Zombie.h
+++ b/src/Mobs/Zombie.h
@@ -10,17 +10,15 @@ class cZombie :
 	public cAggressiveMonster
 {
 	typedef cAggressiveMonster super;
-	
+
 public:
 	cZombie(bool a_IsVillagerZombie);
 
 	CLASS_PROTODEF(cZombie)
-	
-	virtual void GetDrops(cItems & a_Drops, cEntity * a_Killer = nullptr) override;
-	virtual void MoveToPosition(const Vector3d & a_Position) override;
 
+	virtual void GetDrops(cItems & a_Drops, cEntity * a_Killer = nullptr) override;
 	virtual bool IsUndead(void) override { return true; }
-	
+
 	bool IsVillagerZombie(void) const { return m_IsVillagerZombie; }
 	bool IsConverting    (void) const { return m_IsConverting; }