8 files changed, 224 insertions, 265 deletions

diff --git a/src/Entities/Entity.cpp b/src/Entities/Entity.cpp
index 51cee248e..b87cf51a6 100644
--- a/src/Entities/Entity.cpp
+++ b/src/Entities/Entity.cpp
@@ -247,7 +247,6 @@ void cEntity::TakeDamage(eDamageType a_DamageType, cEntity * a_Attacker, int a_R
 	if (a_Attacker != nullptr)
 	{
 		Heading = a_Attacker->GetLookVector() * (a_Attacker->IsSprinting() ? 16 : 11);
-		Heading.y = 1.6;
 	}
 
 	TDI.Knockback = Heading * a_KnockbackAmount;
diff --git a/src/Entities/Player.h b/src/Entities/Player.h
index 8cce4e202..4b8c01dc4 100644
--- a/src/Entities/Player.h
+++ b/src/Entities/Player.h
@@ -122,7 +122,7 @@ public:
 	double GetEyeHeight(void) const;  // tolua_export
 	Vector3d GetEyePosition(void) const;  // tolua_export
 	virtual bool IsOnGround(void) const override { return m_bTouchGround; }
-	inline double GetStance(void) const { return GetPosY() + 1.62; }  // tolua_export  // TODO: Proper stance when crouching etc.
+	inline double GetStance(void) const { return m_Stance; }  // tolua_export
 	inline cInventory &       GetInventory(void)       { return m_Inventory; }  // tolua_export
 	inline const cInventory & GetInventory(void) const { return m_Inventory; }
 
diff --git a/src/Mobs/Monster.cpp b/src/Mobs/Monster.cpp
index 70c3b096b..01077bcdd 100644
--- a/src/Mobs/Monster.cpp
+++ b/src/Mobs/Monster.cpp
@@ -75,10 +75,8 @@ cMonster::cMonster(const AString & a_ConfigName, eMonsterType a_MobType, const A
 	, m_EMPersonality(AGGRESSIVE)
 	, m_Target(nullptr)
 	, m_Path(nullptr)
-	, m_PathStatus(ePathFinderStatus::PATH_NOT_FOUND)
 	, m_IsFollowingPath(false)
 	, m_GiveUpCounter(0)
-	, m_bMovingToDestination(false)
 	, m_LastGroundHeight(POSY_TOINT)
 	, m_JumpCoolDown(0)
 	, m_IdleInterval(0)
@@ -122,101 +120,124 @@ void cMonster::SpawnOn(cClientHandle & a_Client)
 
 
 
-void cMonster::TickPathFinding(cChunk & a_Chunk)
+bool cMonster::TickPathFinding(cChunk & a_Chunk)
 {
-
-	if (m_Path == nullptr)
+	if (!m_IsFollowingPath)
 	{
-		Vector3d position = GetPosition();
-		Vector3d Dest = m_FinalDestination;
-
-		// Can someone explain why are these two NOT THE SAME???
-		// m_Path = new cPath(GetWorld(), GetPosition(), m_FinalDestination, 30);
-		m_Path = new cPath(a_Chunk, Vector3d(floor(position.x), floor(position.y), floor(position.z)), Vector3d(floor(Dest.x), floor(Dest.y), floor(Dest.z)), 20);
-
+		return false;
+	}
 
-		m_IsFollowingPath = false;
+	if (ReachedFinalDestination())
+	{
+		StopMovingToPosition();
+		return false;
 	}
-	m_PathStatus = m_Path->Step(a_Chunk);
-	switch (m_PathStatus)
+
+	if (m_Path == nullptr)
 	{
+		m_Path = new cPath(a_Chunk, GetPosition().Floor(), m_FinalDestination.Floor(), 20);
+	}
 
+	switch (m_Path->Step(a_Chunk))
+	{
 		case ePathFinderStatus::PATH_NOT_FOUND:
 		{
-			ResetPathFinding();
+			StopMovingToPosition();  // Give up pathfinding to that destination
 			break;
 		}
-
-
 		case ePathFinderStatus::CALCULATING:
 		{
-			m_Destination = GetPosition();
+			// Pathfinder needs more time
 			break;
 		}
-
-
 		case ePathFinderStatus::PATH_FOUND:
 		{
-			if (ReachedDestination() || !m_IsFollowingPath)
+			if (--m_GiveUpCounter == 0)
 			{
-				m_Destination = m_Path->GetNextPoint();
-				m_IsFollowingPath = true;
-				m_GiveUpCounter = 40;  // Give up after 40 ticks (2 seconds) if failed to reach m_Dest.
+				ResetPathFinding();  // Try to calculate a path again
+				return false;
 			}
-			if (m_Path->IsLastPoint())
+			else if (!m_Path->IsLastPoint() && (m_Path->IsFirstPoint() || ReachedDestination()))  // Have we arrived at the next cell, as denoted by m_Destination?
 			{
-				ResetPathFinding();
+				m_Destination = Vector3d(0.5, 0, 0.5) + m_Path->GetNextPoint();
+				m_GiveUpCounter = 40;  // Give up after 40 ticks (2 seconds) if failed to reach m_Destination.
 			}
-			break;
-
+			return true;
 		}
 	}
-}
-
 
-
-
-
-/* Currently, the mob will only start moving to a new position after the position it is
-currently going to is reached. */
-void cMonster::MoveToPosition(const Vector3d & a_Position)
-{
-	m_FinalDestination = a_Position;
-	m_bMovingToDestination = true;
+	return false;
 }
 
 
 
 
 
-void cMonster::StopMovingToPosition()
+void cMonster::MoveToWayPoint(cChunk & a_Chunk)
 {
-	m_bMovingToDestination = false;
-	ResetPathFinding();
-}
-
-
-
+	if (m_JumpCoolDown == 0)
+	{
+		// We're not moving and waypoint is above us, it means we are hitting something and we should jump.
+		if ((GetSpeedX() < 0.1) && (GetSpeedY() < 0.1) && DoesPosYRequireJump(FloorC(m_Destination.y)))
+		{
+			if (IsOnGround() || 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!!
+				SetSpeedX(3.2 * (m_Destination.x - GetPosition().x));  // Move forward in a preset speed.
+				SetSpeedZ(3.2 * (m_Destination.z - GetPosition().z));  // The numbers were picked based on trial and error and 1.6 and 3.2 are perfect.
+			}
+		}
+	}
+	else
+	{
+		--m_JumpCoolDown;
+	}
 
+	Vector3d Distance = m_Destination - GetPosition();
+	if ((Distance.x != 0) || (Distance.z != 0))
+	{
+		Distance.y = 0;
+		Distance.Normalize();
 
-bool cMonster::IsCoordinateInTraversedList(Vector3i a_Coords)
-{
-	return (std::find(m_TraversedCoordinates.begin(), m_TraversedCoordinates.end(), a_Coords) != m_TraversedCoordinates.end());
+		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_Destinations, hence
+		better pathfinding. */
+		Distance *= 0.5;
+		AddSpeedX(Distance.x);
+		AddSpeedZ(Distance.z);
+	}
 }
 
 
 
 
 
-/* No one should call this except the pathfinder orthe monster tick or StopMovingToPosition.
-Resets the pathfinder, usually starting a brand new path, unless called from StopMovingToPosition. */
-void cMonster::ResetPathFinding(void)
+void cMonster::MoveToPosition(const Vector3d & a_Position)
 {
-	if (m_Path != nullptr)
+	if ((m_FinalDestination - a_Position).Length() > 0.25)
 	{
-		delete m_Path;
-		m_Path = nullptr;
+		ResetPathFinding();
 
+		m_FinalDestination = a_Position;
+		m_IsFollowingPath = true;
 	}
 }
 
@@ -224,28 +245,23 @@ void cMonster::ResetPathFinding(void)
 
 
 
-bool cMonster::ReachedDestination()
+void cMonster::StopMovingToPosition()
 {
-	if ((m_Destination - GetPosition()).Length() < 0.5f)
-	{
-		return true;
-	}
-
-	return false;
+	m_IsFollowingPath = false;
+	ResetPathFinding();
 }
 
 
 
 
 
-bool cMonster::ReachedFinalDestination()
+void cMonster::ResetPathFinding(void)
 {
-	if ((GetPosition() - m_FinalDestination).Length() <= m_AttackRange)
+	if (m_Path != nullptr)
 	{
-		return true;
+		delete m_Path;
+		m_Path = nullptr;
 	}
-
-	return false;
 }
 
 
@@ -277,83 +293,18 @@ void cMonster::Tick(std::chrono::milliseconds a_Dt, cChunk & a_Chunk)
 		m_Target = nullptr;
 	}
 
-	// Burning in daylight
-	bool WouldBurnRightNow = WouldBurnAt(GetPosition(), *Chunk);  // cached so that we use it twice, spares some cycles.
-	HandleDaylightBurning(*Chunk, WouldBurnRightNow);
-
-
-
-	if (m_bMovingToDestination)
+	// Process the undead burning in daylight
+	HandleDaylightBurning(*Chunk, WouldBurnAt(GetPosition(), *Chunk));
+	if (TickPathFinding(*Chunk))
 	{
-		if (m_bOnGround)
-		{
-			if (m_JumpCoolDown == 0)
-			{
-				if (DoesPosYRequireJump(static_cast<int>(floor(m_Destination.y))))
-				{
-					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_Destination.x - GetPosition().x));  // Move forward in a preset speed.
-					SetSpeedZ(3.2 * (m_Destination.z - GetPosition().z));  // The numbers were picked based on trial and error and 1.6 and 3.2 are perfect.
-				}
-			}
-			else
-			{
-				--m_JumpCoolDown;
-			}
-		}
-
-		TickPathFinding(a_Chunk);
-
-		Vector3d Distance = m_Destination - GetPosition();
-		if (!ReachedDestination() && !ReachedFinalDestination())  // If we haven't reached any sort of destination, move
+		if (m_BurnsInDaylight && WouldBurnAt(m_Destination, *Chunk->GetNeighborChunk(FloorC(m_Destination.x), FloorC(m_Destination.z))) && !IsOnFire() && (m_TicksSinceLastDamaged == 100))
 		{
-			if (--m_GiveUpCounter == 0)
-			{
-				ResetPathFinding();  // Not to be confused with StopMovingToPosition, this just discards the current path and calculates another.
-			}
-			else if (m_BurnsInDaylight && WouldBurnAt(m_Destination, *Chunk) && !WouldBurnRightNow && (m_TicksSinceLastDamaged == 100))
-			{
-				// 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();
-			}
-			else
-			{
-				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_Destinations, hence
-				better pathfinding. */
-				Distance *= 0.5;
-
-				AddSpeedX(Distance.x);
-				AddSpeedZ(Distance.z);
-
-			}
+			// 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();
 		}
-		else if (ReachedFinalDestination())
+		else
 		{
-			StopMovingToPosition();
+			MoveToWayPoint(*Chunk);
 		}
 	}
 
@@ -379,12 +330,11 @@ void cMonster::Tick(std::chrono::milliseconds a_Dt, cChunk & a_Chunk)
 			InStateEscaping(a_Dt);
 			break;
 		}
-
 		case ATTACKING: break;
 	}  // switch (m_EMState)
 
 	BroadcastMovementUpdate();
-	}
+}
 
 
 
@@ -397,32 +347,29 @@ 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_Destination - GetPosition();
+		double Rotation, Pitch;
+		BodyDistance.Normalize();
+		VectorToEuler(BodyDistance.x, BodyDistance.y, BodyDistance.z, Rotation, Pitch);
+		SetYaw(Rotation);
 	}
 }
 
@@ -662,7 +609,7 @@ void cMonster::EventLosePlayer(void)
 
 void cMonster::InStateIdle(std::chrono::milliseconds a_Dt)
 {
-	if (m_bMovingToDestination)
+	if (m_IsFollowingPath)
 	{
 		return;  // Still getting there
 	}
@@ -682,14 +629,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);
 		}
 	}
 }
diff --git a/src/Mobs/Monster.h b/src/Mobs/Monster.h
index 978266165..329b9f399 100644
--- a/src/Mobs/Monster.h
+++ b/src/Mobs/Monster.h
@@ -61,9 +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 void StopMovingToPosition();
-	virtual bool ReachedDestination(void);
 
 	// tolua_begin
 	eMonsterType GetMobType(void) const { return m_MobType; }
@@ -160,24 +160,21 @@ public:
 
 protected:
 
-	/* ======= PATHFINDING ======= */
-
 	/** A pointer to the entity this mobile is aiming to reach */
 	cEntity * m_Target;
 	cPath *  m_Path;  // TODO unique ptr
-	ePathFinderStatus m_PathStatus;
+
+	/** Stores if mobile is currently moving towards the ultimate, final destination */
 	bool m_IsFollowingPath;
-	/* If 0, will give up reaching the next m_Dest and will re-compute path. */
+
+	/* If 0, will give up reaching the next m_Destination and will re-compute path. */
 	int m_GiveUpCounter;
+
 	/** Coordinates of the next position that should be reached */
 	Vector3d m_Destination;
+
 	/** 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;
 
 	/** 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
@@ -185,42 +182,41 @@ 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_Destination has been reached */
+	bool ReachedDestination(void) { return ((m_Destination - 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_Destination 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_Destination
+	*/
+	bool TickPathFinding(cChunk & a_Chunk);
+	void MoveToWayPoint(cChunk & a_Chunk);
 
-	/** 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(cChunk & a_Chunk);
-	/** Finishes a pathfinding task, be it due to failure or something else */
+	/** 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();
+
 	/** 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;
 
diff --git a/src/Mobs/Path.cpp b/src/Mobs/Path.cpp
index 2dd2a3c99..8701dad10 100644
--- a/src/Mobs/Path.cpp
+++ b/src/Mobs/Path.cpp
@@ -7,13 +7,13 @@
 
 #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 5  // Higher means more CPU load but faster path calculations.
+#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
 {
-	Vector3d m_Location;   // Location of the cell in the world.
+	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*.
@@ -36,28 +36,51 @@ bool compareHeuristics::operator()(cPathCell * & a_Cell1, cPathCell * & a_Cell2)
 /* cPath implementation */
 cPath::cPath(
 	cChunk & a_Chunk,
-	const Vector3d & a_StartingPoint, const Vector3d & a_EndingPoint, int a_MaxSteps,
+	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
 
-	m_Chunk = &a_Chunk;
-	m_Source = a_StartingPoint.Floor();
-	m_Destination = a_EndingPoint.Floor();
-
 	if (GetCell(m_Source)->m_IsSolid || GetCell(m_Destination)->m_IsSolid)
 	{
 		m_Status = ePathFinderStatus::PATH_NOT_FOUND;
 		return;
 	}
 
-	m_Status = ePathFinderStatus::CALCULATING;
+	// 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 - m_Chunk->GetPosX() * cChunkDef::Width;
+		int RelZ = m_Destination.z - m_Chunk->GetPosZ() * cChunkDef::Width;
+		bool inwater = false;
+		for (;;)
+		{
+			m_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;
-	m_PointCount = 0;
 
 	ProcessCell(GetCell(a_StartingPoint), nullptr, 0);
 	m_Chunk = nullptr;
@@ -82,6 +105,7 @@ cPath::~cPath()
 ePathFinderStatus cPath::Step(cChunk & a_Chunk)
 {
 	m_Chunk = &a_Chunk;
+
 	if (m_Status != ePathFinderStatus::CALCULATING)
 	{
 		return m_Status;
@@ -103,6 +127,7 @@ ePathFinderStatus cPath::Step(cChunk & a_Chunk)
 			}
 		}
 	}
+
 	m_Chunk = nullptr;
 	return m_Status;
 }
@@ -111,23 +136,32 @@ ePathFinderStatus cPath::Step(cChunk & a_Chunk)
 
 
 
-bool cPath::IsSolid(const Vector3d & a_Location)
+bool cPath::IsSolid(const Vector3i & a_Location)
 {
 	ASSERT(m_Chunk != nullptr);
-	m_Chunk = m_Chunk->GetNeighborChunk(a_Location.x, a_Location.z);
-	if ((m_Chunk == nullptr) || (!m_Chunk->IsValid()))
+
+	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 + Vector3d(0, 1, 0))->m_IsSolid = true;  // Mobs will always think that the fence is 2 blocks high and therefore won't jump over.
+		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);
 }
 
@@ -152,11 +186,10 @@ bool cPath::Step_Internal()
 	{
 		do
 		{
-			AddPoint(CurrentCell->m_Location + Vector3d(0.5, 0, 0.5));  // Populate the cPath with points.
+			m_PathPoints.push_back(CurrentCell->m_Location);  // Populate the cPath with points.
 			CurrentCell = CurrentCell->m_Parent;
 		} while (CurrentCell != nullptr);
 
-		m_CurrentPoint = -1;
 		FinishCalculation(ePathFinderStatus::PATH_FOUND);
 		return true;
 	}
@@ -167,10 +200,10 @@ bool cPath::Step_Internal()
 	int i;
 	for (i = -1; i <= 1; ++i)
 	{
-		ProcessIfWalkable(CurrentCell->m_Location + Vector3d(1, i, 0),  CurrentCell, 10);
-		ProcessIfWalkable(CurrentCell->m_Location + Vector3d(-1, i, 0), CurrentCell, 10);
-		ProcessIfWalkable(CurrentCell->m_Location + Vector3d(0, i, 1),  CurrentCell, 10);
-		ProcessIfWalkable(CurrentCell->m_Location + Vector3d(0, i, -1), CurrentCell, 10);
+		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.
@@ -180,18 +213,17 @@ bool cPath::Step_Internal()
 		for (z = -1; z <= 1; z += 2)
 		{
 			// This condition prevents diagonal corner cutting.
-			if (!GetCell(CurrentCell->m_Location + Vector3d(x, 0, 0))->m_IsSolid && !GetCell(CurrentCell->m_Location + Vector3d(0, 0, z))->m_IsSolid)
+			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 + Vector3d(x, -1, 0))->m_IsSolid && GetCell(CurrentCell->m_Location + Vector3d(0, -1, z))->m_IsSolid)
+				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 + Vector3d(x, 0, z), CurrentCell, 14);  // 14 is a good enough approximation of sqrt(10 + 10).
+					ProcessIfWalkable(CurrentCell->m_Location + Vector3i(x, 0, z), CurrentCell, 14);  // 14 is a good enough approximation of sqrt(10 + 10).
 				}
 			}
 		}
 	}
 
-
 	return false;
 }
 
@@ -257,10 +289,10 @@ si::setBlock((Ret)->m_Location.x, (Ret)->m_Location.y, (Ret)->m_Location.z, debu
 
 
 
-void cPath::ProcessIfWalkable(const Vector3d & a_Location, cPathCell * a_Parent, int a_Cost)
+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 + Vector3d(0, -1, 0))->m_IsSolid && !GetCell(a_Location + Vector3d(0, 1, 0))->m_IsSolid)
+	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);
 	}
@@ -298,7 +330,7 @@ void cPath::ProcessCell(cPathCell * a_Cell, cPathCell * a_Caller, int a_GDelta)
 			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 = std::sqrt((a_Cell->m_Location.x - m_Destination.x) * (a_Cell->m_Location.x - m_Destination.x) * 100 + (a_Cell->m_Location.y - m_Destination.y) * (a_Cell->m_Location.y - m_Destination.y) * 100 + (a_Cell->m_Location.z - m_Destination.z) * (a_Cell->m_Location.z - m_Destination.z) * 100);
+			a_Cell->m_H = static_cast<decltype(a_Cell->m_H)>((a_Cell->m_Location - m_Destination).Length() * 10);
 		#endif
 
 		#if HEURISTICS_ONLY == 1
@@ -326,7 +358,7 @@ void cPath::ProcessCell(cPathCell * a_Cell, cPathCell * a_Caller, int a_GDelta)
 
 
 
-cPathCell * cPath::GetCell(const Vector3d & a_Location)
+cPathCell * cPath::GetCell(const Vector3i & a_Location)
 {
 	// Create the cell in the hash table if it's not already there.
 	cPathCell * Cell;
@@ -349,14 +381,3 @@ cPathCell * cPath::GetCell(const Vector3d & a_Location)
 		return m_Map[a_Location];
 	}
 }
-
-
-
-
-
-// Add the next point in the final path.
-void cPath::AddPoint(Vector3d a_Vector)
-{
-	m_PathPoints.push_back(a_Vector);
-	++m_PointCount;
-}
diff --git a/src/Mobs/Path.h b/src/Mobs/Path.h
index 1bce0ace0..9e893f1d7 100644
--- a/src/Mobs/Path.h
+++ b/src/Mobs/Path.h
@@ -53,7 +53,7 @@ public:
 	@param a_MaxSteps The maximum steps before giving up. */
 	cPath(
 		cChunk & a_Chunk,
-		const Vector3d & a_StartingPoint, const Vector3d & a_EndingPoint, int a_MaxSteps,
+		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
 	);
@@ -66,35 +66,39 @@ public:
 
 	/* Point retrieval functions, inlined for performance. */
 	/** Returns the next point in the path. */
-	inline Vector3d GetNextPoint()
+	inline Vector3i GetNextPoint()
 	{
 		ASSERT(m_Status == ePathFinderStatus::PATH_FOUND);
-		return m_PathPoints[m_PointCount - 1 - (++m_CurrentPoint)];
+		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);
-		ASSERT(m_CurrentPoint != -1);  // You must call getFirstPoint at least once before calling this.
-		return (m_CurrentPoint == m_PointCount - 1);
+		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 Vector3d GetPoint(int a_index)
+	inline Vector3i GetPoint(size_t a_index)
 	{
 		ASSERT(m_Status == ePathFinderStatus::PATH_FOUND);
-		ASSERT(a_index < m_PointCount);
-		return m_PathPoints[m_PointCount - 1 - a_index];
+		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_PointCount;
+		return m_PathPoints.size();
 	}
 
 	struct VectorHasher
 	{
-		std::size_t operator()(const Vector3d & a_Vector) const
+		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;
@@ -110,7 +114,7 @@ public:
 private:
 
 	/* General */
-	bool IsSolid(const Vector3d & a_Location);  // Query our hosting world and ask it if there's a solid at a_location.
+	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.
@@ -118,27 +122,25 @@ private:
 	/* Openlist and closedlist management */
 	void OpenListAdd(cPathCell * a_Cell);
 	cPathCell * OpenListPop();
-	void ProcessIfWalkable(const Vector3d &a_Location, cPathCell * a_Parent, int a_Cost);
+	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 Vector3d & a_location);
+	cPathCell * GetCell(const Vector3i & a_location);
 
 	/* Pathfinding fields */
 	std::priority_queue<cPathCell *,  std::vector<cPathCell *>,  compareHeuristics> m_OpenList;
-	std::unordered_map<Vector3d,  cPathCell *, VectorHasher> m_Map;
-	Vector3d m_Destination;
-	Vector3d m_Source;
+	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 */
-	int m_PointCount;
-	int m_CurrentPoint;
-	std::vector<Vector3d> m_PathPoints;
-	void AddPoint(Vector3d a_Vector);
+	size_t m_CurrentPoint;
+	std::vector<Vector3i> m_PathPoints;
 
 	/* Interfacing with the world */
 	cChunk * m_Chunk;  // Only valid inside Step()!
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/Protocol/Protocol18x.cpp b/src/Protocol/Protocol18x.cpp
index 0baae00de..1fc0c53a9 100644
--- a/src/Protocol/Protocol18x.cpp
+++ b/src/Protocol/Protocol18x.cpp
@@ -2310,7 +2310,7 @@ void cProtocol180::HandlePacketPlayerPos(cByteBuffer & a_ByteBuffer)
 	HANDLE_READ(a_ByteBuffer, ReadBEDouble, double, PosY);
 	HANDLE_READ(a_ByteBuffer, ReadBEDouble, double, PosZ);
 	HANDLE_READ(a_ByteBuffer, ReadBool,     bool,   IsOnGround);
-	m_Client->HandlePlayerPos(PosX, PosY, PosZ, PosY + 1.62, IsOnGround);
+	m_Client->HandlePlayerPos(PosX, PosY, PosZ, PosY + (m_Client->GetPlayer()->IsCrouched() ? 1.54 : 1.62), IsOnGround);
 }