4 files changed, 46 insertions, 8 deletions

diff --git a/src/control/PathFind.cpp b/src/control/PathFind.cpp
index e9276dde..af4754b9 100644
--- a/src/control/PathFind.cpp
+++ b/src/control/PathFind.cpp
@@ -9,6 +9,8 @@
 
 CPathFind &ThePaths = *(CPathFind*)0x8F6754;
 
+WRAPPER bool CPedPath::CalcPedRoute(uint8, CVector, CVector, CVector*, int16*, int16) { EAXJMP(0x42E680); }
+
 enum
 {
 	NodeTypeExtern = 1,
@@ -1197,9 +1199,9 @@ CPathFind::FindNextNodeWandering(uint8 type, CVector coors, CPathNode **lastNode
 		CTreadable *obj = FindRoadObjectClosestToCoors(coors, type);
 		float nodeDist = 1000000000.0f;
 		for(i = 0; i < 12; i++){
-			if(obj->m_nodeIndices[i] < 0)
+			if(obj->m_nodeIndices[type][i] < 0)
 				break;
-			float dist = (coors - m_pathNodes[obj->m_nodeIndices[type][i]].pos).Magnitude2D();
+			float dist = (coors - m_pathNodes[obj->m_nodeIndices[type][i]].pos).MagnitudeSqr();
 			if(dist < nodeDist){
 				nodeDist = dist;
 				node = &m_pathNodes[obj->m_nodeIndices[type][i]];
@@ -1207,12 +1209,12 @@ CPathFind::FindNextNodeWandering(uint8 type, CVector coors, CPathNode **lastNode
 		}
 	}
 
-	CVector2D vCurDir(Cos(curDir*PI/4.0f), Sin(curDir*PI/4.0f));
+	CVector2D vCurDir(Sin(curDir*PI/4.0f), Cos(curDir * PI / 4.0f));
 	*nextNode = 0;
 	float bestDot = -999999.0f;
 	for(i = 0; i < node->numLinks; i++){
 		int next = m_connections[node->firstLink+i];
-		if(node->bDisabled || m_pathNodes[next].bDisabled)
+		if(!node->bDisabled && m_pathNodes[next].bDisabled)
 			continue;
 		CVector pedCoors = coors;
 		pedCoors.z += 1.0f;
@@ -1221,9 +1223,9 @@ CPathFind::FindNextNodeWandering(uint8 type, CVector coors, CPathNode **lastNode
 		if(!CWorld::GetIsLineOfSightClear(pedCoors, nodeCoors, true, false, false, false, false, false))
 			continue;
 		CVector2D nodeDir = m_pathNodes[next].pos - node->pos;
-		nodeDir /= nodeDir.Magnitude();
+		nodeDir.Normalise();
 		float dot = DotProduct2D(nodeDir, vCurDir);
-		if(dot > bestDot){
+		if(dot >= bestDot){
 			*nextNode = &m_pathNodes[next];
 			bestDot = dot;
 
diff --git a/src/control/PathFind.h b/src/control/PathFind.h
index de30d70b..e3058959 100644
--- a/src/control/PathFind.h
+++ b/src/control/PathFind.h
@@ -4,6 +4,11 @@
 
 class CVehicle;
 
+class CPedPath {
+public:
+	static bool CalcPedRoute(uint8, CVector, CVector, CVector*, int16*, int16);
+};
+
 enum
 {
 	PATH_CAR = 0,
diff --git a/src/peds/Ped.cpp b/src/peds/Ped.cpp
index d9ddffc8..5a2472e0 100644
--- a/src/peds/Ped.cpp
+++ b/src/peds/Ped.cpp
@@ -53,7 +53,6 @@
 WRAPPER void CPed::SpawnFlyingComponent(int, int8) { EAXJMP(0x4EB060); }
 WRAPPER void CPed::SetPedPositionInCar(void) { EAXJMP(0x4D4970); }
 WRAPPER void CPed::SetMoveAnim(void) { EAXJMP(0x4C5A40); }
-WRAPPER void CPed::SetFollowPath(CVector) { EAXJMP(0x4D2EA0); }
 WRAPPER void CPed::StartFightDefend(uint8, uint8, uint8) { EAXJMP(0x4E7780); }
 WRAPPER void CPed::ServiceTalking(void) { EAXJMP(0x4E5870); }
 WRAPPER void CPed::UpdatePosition(void) { EAXJMP(0x4C7A00); }
@@ -14879,6 +14878,37 @@ CPed::SetSolicit(uint32 time)
 	}
 }
 
+bool
+CPed::SetFollowPath(CVector dest)
+{
+	if (m_nPedState == PED_FOLLOW_PATH)
+		return false;
+
+	if (FindPlayerPed() != this)
+		return false;
+
+	if ((dest - GetPosition()).Magnitude() <= 2.0f)
+		return false;
+
+	CVector pointPoses[7];
+	int16 pointsFound;
+	CPedPath::CalcPedRoute(0, GetPosition(), dest, pointPoses, &pointsFound, 7);
+	for(int i = 0; i < pointsFound; i++) {
+		m_stPathNodeStates[i].x = pointPoses[i].x;
+		m_stPathNodeStates[i].y = pointPoses[i].y;
+	}
+
+	m_nCurPathNode = 0;
+	m_nPathNodes = pointsFound;
+	if (m_nPathNodes < 1)
+		return false;
+
+	SetStoredState();
+	m_nPedState = PED_FOLLOW_PATH;
+	SetMoveState(PEDMOVE_WALK);
+	return true;
+}
+
 class CPed_ : public CPed
 {
 public:
@@ -15092,4 +15122,5 @@ STARTPATCHES
 	InjectHook(0x4D7BC0, &CPed::SetRadioStation, PATCH_JUMP);
 	InjectHook(0x4C7FF0, &CPed::ProcessBuoyancy, PATCH_JUMP);
 	InjectHook(0x4D6620, &CPed::SetSolicit, PATCH_JUMP);
+	InjectHook(0x4D2EA0, &CPed::SetFollowPath, PATCH_JUMP);
 ENDPATCHES
\ No newline at end of file
diff --git a/src/peds/Ped.h b/src/peds/Ped.h
index 90b3ca0f..c1ac7cfd 100644
--- a/src/peds/Ped.h
+++ b/src/peds/Ped.h
@@ -574,7 +574,7 @@ public:
 	void SetPointGunAt(CEntity*);
 	bool Seek(void);
 	bool SetWanderPath(int8);
-	void SetFollowPath(CVector);
+	bool SetFollowPath(CVector);
 	void ClearAttackByRemovingAnim(void);
 	void SetStoredState(void);
 	void StopNonPartialAnims(void);