finished CPathFind

src/control/PathFind.h

@@ -2,24 +2,35 @@
 
 #include "Treadable.h"
 
+enum
+{
+	PATH_CAR = 0,
+	PATH_PED = 1,
+};
+
 struct CPathNode
 {
 	CVector pos;
-	CPathNode *prev;	//?
+	CPathNode *prev;
 	CPathNode *next;
-	int16 unknown;
+	int16 distance;		// in path search
 	int16 objectIndex;
 	int16 firstLink;
 	uint8 numLinks;
-	uint8 flags;
+
+	uint8 unkBits : 2;
+	uint8 bDeadEnd : 1;
+	uint8 bDisabled : 1;
+	uint8 bBetweenLevels : 1;
+
 	uint8 group;
-/*	VC:
+/*	For reference VC:
-	int16 unk1;
+	int16 prevIndex;
 	int16 nextIndex;
 	int16 x;
 	int16 y;
 	int16 z;
-	int16 unknown;
+	int16 distance;
 	int16 firstLink;
 	int8 width;
 	int8 group;
@@ -40,6 +51,15 @@ struct CPathNode
 */
 };
 
+union CConnectionFlags
+{
+	uint8 flags;
+	struct {
+		uint8 bCrossesRoad : 1;
+		uint8 bTrafficLight : 1;
+	};
+};
+
 struct CCarPathLink
 {
 	float posX;
@@ -50,10 +70,9 @@ struct CCarPathLink
 	int8 numLeftLanes;
 	int8 numRightLanes;
 	int8 trafficLightType;
-	int8 field15;
+
-	// probably only padding
+	uint8 bBridgeLights : 1;
-	int8 field16;
+	// more?
-	int8 field17;
 };
 
 struct CPathInfoForObject
@@ -80,8 +99,6 @@ struct CTempNode
 	int8 numLeftLanes;
 	int8 numRightLanes;
 	int8 linkState;
-	// probably padding
-	int8 field1B;
 };
 
 struct CTempDetachedNode	// unused
@@ -102,41 +119,65 @@ public:
 	uint8 m_distances[20400];
 	int16 m_carPathConnections[20400];
 */
-	CPathNode m_pathNodes[4930];
+	CPathNode m_pathNodes[NUM_PATHNODES];
-	CCarPathLink m_carPathLinks[2076];
+	CCarPathLink m_carPathLinks[NUM_CARPATHLINKS];
-	CTreadable *m_mapObjects[1250];
+	CTreadable *m_mapObjects[NUM_MAPOBJECTS];
-	uint8 m_objectFlags[1250];
+	uint8 m_objectFlags[NUM_MAPOBJECTS];
-	int16 m_connections[10260];
+	int16 m_connections[NUM_PATHCONNECTIONS];
-	int16 m_distances[10260];
+	int16 m_distances[NUM_PATHCONNECTIONS];
-	uint8 m_connectionFlags[10260];
+	CConnectionFlags m_connectionFlags[NUM_PATHCONNECTIONS];
-	int16 m_carPathConnections[10260];
+	int16 m_carPathConnections[NUM_PATHCONNECTIONS];
 	int32 m_numPathNodes;
 	int32 m_numCarPathNodes;
 	int32 m_numPedPathNodes;
 	int16 m_numMapObjects;
 	int16 m_numConnections;
 	int32 m_numCarPathLinks;
-	int32 h;
+	int32 unk;
 	uint8 m_numGroups[2];
-	CPathNode m_aExtraPaths[872];
+	CPathNode m_searchNodes[512];
 
+	void Init(void);
+	void AllocatePathFindInfoMem(int16 numPathGroups);
+	void RegisterMapObject(CTreadable *mapObject);
+	void StoreNodeInfoPed(int16 id, int16 node, int8 type, int8 next, int16 x, int16 y, int16 z, int16 width, bool crossing);
+	void StoreNodeInfoCar(int16 id, int16 node, int8 type, int8 next, int16 x, int16 y, int16 z, int16 width, int8 numLeft, int8 numRight);
+	void CalcNodeCoors(int16 x, int16 y, int16 z, int32 id, CVector *out);
+	bool LoadPathFindData(void);
 	void PreparePathData(void);
 	void CountFloodFillGroups(uint8 type);
 	void PreparePathDataForType(uint8 type, CTempNode *tempnodes, CPathInfoForObject *objectpathinfo,
 		float unk, CTempDetachedNode *detachednodes, int unused);
-	void CalcNodeCoors(int16 x, int16 y, int16 z, int32 id, CVector *out);
-	void StoreNodeInfoPed(int16 id, int16 node, int8 type, int8 next, int16 x, int16 y, int16 z, int16 width, bool crossing);
-	void StoreNodeInfoCar(int16 id, int16 node, int8 type, int8 next, int16 x, int16 y, int16 z, int16 width, int8 numLeft, int8 numRight);
-	void RegisterMapObject(CTreadable *mapObject);
-	int32 FindNodeClosestToCoors(CVector coors, uint8 type, float distLimit, bool disabled, bool betweenLevels);
-	CPathNode** FindNextNodeWandering(uint8, CVector, CPathNode**, CPathNode**, uint8, uint8*);
-	bool NewGenerateCarCreationCoors(float spawnX, float spawnY, float frontX, float frontY, float preferredDistance, float angleLimit /* angle limit between camera direction and vector to spawn */, bool invertAngleLimitTest, CVector* pSpawnPosition, int32* pNode1, int32* pNode2, float* pPositionBetweenNodes, bool ignoreSwitchedOff);
-	bool TestCoorsCloseness(CVector pos1, bool, CVector pos2);
 
 	bool IsPathObject(int id) { return id < PATHNODESIZE && (InfoForTileCars[id*12].type != 0 || InfoForTilePeds[id*12].type != 0); }
 
+	float CalcRoadDensity(float x, float y);
+	bool TestForPedTrafficLight(CPathNode *n1, CPathNode *n2);
+	bool TestCrossesRoad(CPathNode *n1, CPathNode *n2);
+	void AddNodeToList(CPathNode *node, int32 listId);
+	void RemoveNodeFromList(CPathNode *node);
+	void RemoveBadStartNode(CVector pos, CPathNode **nodes, int16 *n);
 	void SetLinksBridgeLights(float, float, float, float, bool);
+	void SwitchOffNodeAndNeighbours(int32 nodeId, bool disable);
+	void SwitchRoadsOffInArea(float x1, float x2, float y1, float y2, float z1, float z2, bool disable);
+	void SwitchPedRoadsOffInArea(float x1, float x2, float y1, float y2, float z1, float z2, bool disable);
+	void SwitchRoadsInAngledArea(float x1, float y1, float z1, float x2, float y2, float z2, float length, uint8 type, uint8 enable);
+	void MarkRoadsBetweenLevelsNodeAndNeighbours(int32 nodeId);
+	void MarkRoadsBetweenLevelsInArea(float x1, float x2, float y1, float y2, float z1, float z2);
+	void MarkPedRoadsBetweenLevelsInArea(float x1, float x2, float y1, float y2, float z1, float z2);
+	int32 FindNodeClosestToCoors(CVector coors, uint8 type, float distLimit, bool ignoreDisabled = false, bool ignoreBetweenLevels = false);
+	int32 FindNodeClosestToCoorsFavourDirection(CVector coors, uint8 type, float dirX, float dirY);
+	float FindNodeOrientationForCarPlacement(int32 nodeId);
+	float FindNodeOrientationForCarPlacementFacingDestination(int32 nodeId, float x, float y, bool towards);
+	bool NewGenerateCarCreationCoors(float x, float y, float dirX, float dirY, float spawnDist, float angleLimit, bool forward, CVector *pPosition, int32 *pNode1, int32 *pNode2, float *pPositionBetweenNodes, bool ignoreDisabled = false);
+	bool GeneratePedCreationCoors(float x, float y, float minDist, float maxDist, float minDistOffScreen, float maxDistOffScreen, CVector *pPosition, int32 *pNode1, int32 *pNode2, float *pPositionBetweenNodes, CMatrix *camMatrix);
+	CTreadable *FindRoadObjectClosestToCoors(CVector coors, uint8 type);
+	void FindNextNodeWandering(uint8, CVector, CPathNode**, CPathNode**, uint8, uint8*);
+	void DoPathSearch(uint8 type, CVector start, int32 startNodeId, CVector target, CPathNode **nodes, int16 *numNodes, int16 maxNumNodes, CVehicle *vehicle, float *dist, float distLimit, int32 forcedTargetNode);
+	bool TestCoorsCloseness(CVector target, uint8 type, CVector start);
+	void Save(uint8 *buffer, uint32 *length);
+	void Load(uint8 *buffer, uint32 length);
 };
-static_assert(sizeof(CPathFind) == 0x4c8f4, "CPathFind: error");
+static_assert(sizeof(CPathFind) == 0x49bf4, "CPathFind: error");
 
 extern CPathFind &ThePaths;

src/core/General.h

@@ -74,6 +74,7 @@ public:
 		return result;
 	}
 
+	// Returns an angle such that x2/y2 looks at x1/y1 with its forward vector if rotated by that angle
 	static float GetRadianAngleBetweenPoints(float x1, float y1, float x2, float y2)
 	{
 		float x = x2 - x1;

src/core/config.h

@@ -33,6 +33,12 @@ enum Config {
 
 	NUMTEMPOBJECTS = 30,
 
+	// Path data
+	NUM_PATHNODES = 4930,
+	NUM_CARPATHLINKS = 2076,
+	NUM_MAPOBJECTS = 1250,
+	NUM_PATHCONNECTIONS = 10260,
+
 	// Link list lengths
 	// TODO: alpha list
 	NUMCOLCACHELINKS = 200,

src/entities/Physical.cpp

@@ -11,6 +11,7 @@
 #include "ParticleObject.h"
 #include "Particle.h"
 #include "SurfaceTable.h"
+#include "PathFind.h"
 #include "CarCtrl.h"
 #include "DMAudio.h"
 #include "Automobile.h"
@@ -56,8 +57,8 @@ CPhysical::CPhysical(void)
 	m_phy_flagA80 = false;
 
 	m_fDistanceTravelled = 0.0f;
-	m_pedTreadable = nil;
+	m_treadable[PATH_CAR] = nil;
-	m_carTreadable = nil;
+	m_treadable[PATH_PED] = nil;
 
 	m_phy_flagA10 = false;
 	m_phy_flagA20 = false;
@@ -267,16 +268,16 @@ CPhysical::AddCollisionRecord_Treadable(CEntity *ent)
 {
 	if(ent->IsBuilding() && ((CBuilding*)ent)->GetIsATreadable()){
 		CTreadable *t = (CTreadable*)ent;
-		if(t->m_nodeIndicesPeds[0] >= 0 ||
+		if(t->m_nodeIndices[PATH_PED][0] >= 0 ||
-		   t->m_nodeIndicesPeds[1] >= 0 ||
+		   t->m_nodeIndices[PATH_PED][1] >= 0 ||
-		   t->m_nodeIndicesPeds[2] >= 0 ||
+		   t->m_nodeIndices[PATH_PED][2] >= 0 ||
-		   t->m_nodeIndicesPeds[3] >= 0)
+		   t->m_nodeIndices[PATH_PED][3] >= 0)
-			m_pedTreadable = t;
+			m_treadable[PATH_PED] = t;
-		if(t->m_nodeIndicesCars[0] >= 0 ||
+		if(t->m_nodeIndices[PATH_CAR][0] >= 0 ||
-		   t->m_nodeIndicesCars[1] >= 0 ||
+		   t->m_nodeIndices[PATH_CAR][1] >= 0 ||
-		   t->m_nodeIndicesCars[2] >= 0 ||
+		   t->m_nodeIndices[PATH_CAR][2] >= 0 ||
-		   t->m_nodeIndicesCars[3] >= 0)
+		   t->m_nodeIndices[PATH_CAR][3] >= 0)
-			m_carTreadable = t;
+			m_treadable[PATH_CAR] = t;
 	}
 }
 

src/entities/Physical.h

@@ -19,8 +19,7 @@ public:
 
 	int32 m_audioEntityId;
   float unk1;
-	CTreadable *m_carTreadable;
+	CTreadable *m_treadable[2];	// car and ped
-	CTreadable *m_pedTreadable;
 	uint32 m_nLastTimeCollided;
 	CVector m_vecMoveSpeed;		// velocity
 	CVector m_vecTurnSpeed;		// angular velocity

src/entities/Treadable.h

@@ -8,8 +8,7 @@ public:
 	static void *operator new(size_t);
 	static void operator delete(void*, size_t);
 
-	int16 m_nodeIndicesCars[12];
+	int16 m_nodeIndices[2][12];	// first car, then ped
-	int16 m_nodeIndicesPeds[12];
 
 	bool GetIsATreadable(void) { return true; }
 };

src/math/Vector2D.h

@@ -18,7 +18,7 @@ public:
 			x *= invsqrt;
 			y *= invsqrt;
 		}else
-			x = 0.0f;
+			x = 1.0f;
 	}
 	const CVector2D &operator+=(CVector2D const &right) {
 		x += right.x;
@@ -52,6 +52,9 @@ public:
 	CVector2D operator*(float t) const {
 		return CVector2D(x*t, y*t);
 	}
+	CVector2D operator/(float t) const {
+		return CVector2D(x/t, y/t);
+	}
 };
 
 inline float
@@ -65,3 +68,26 @@ CrossProduct2D(const CVector2D &v1, const CVector2D &v2)
 {
 	return v1.x*v2.y - v1.y*v2.x;
 }
+
+inline float
+Distance2D(const CVector2D &v, float x, float y)
+{
+	return Sqrt((v.x-x)*(v.x-x) + (v.y-y)*(v.y-y));
+}
+
+inline float
+DistanceSqr2D(const CVector2D &v, float x, float y)
+{
+	return (v.x-x)*(v.x-x) + (v.y-y)*(v.y-y);
+}
+
+inline void
+NormalizeXY(float &x, float &y)
+{
+	float l  = Sqrt(x*x + y*y);
+	if(l != 0.0f){
+		x /= l;
+		y /= l;
+	}else
+		x = 1.0f;
+}

src/peds/Ped.cpp

@@ -3856,7 +3856,7 @@ CPed::SetWanderPath(int8 pathStateDest)
 			if (pathStateDest == 0)
 				pathStateDest = CGeneral::GetRandomNumberInRange(1, 7);
 
-			ThePaths.FindNextNodeWandering(1, GetPosition(), &m_pNextPathNode, &m_pLastPathNode,
+			ThePaths.FindNextNodeWandering(PATH_PED, GetPosition(), &m_pNextPathNode, &m_pLastPathNode,
 				m_nPathState, &nextPathState);
 
 			// Circular loop until we find a node for current m_nPathState
@@ -3869,7 +3869,7 @@ CPed::SetWanderPath(int8 pathStateDest)
 					SetIdle();
 					return false;
 				}
-				ThePaths.FindNextNodeWandering(1, GetPosition(), &m_pNextPathNode, &m_pLastPathNode,
+				ThePaths.FindNextNodeWandering(PATH_PED, GetPosition(), &m_pNextPathNode, &m_pLastPathNode,
 					m_nPathState, &nextPathState);
 			}
 

src/render/Renderer.cpp

@@ -200,9 +200,9 @@ CRenderer::RenderRoads(void)
 			if(gbShowCarRoadGroups || gbShowPedRoadGroups){
 				int ind = 0;
 				if(gbShowCarRoadGroups)
-					ind += ThePaths.m_pathNodes[t->m_nodeIndicesCars[0]].group;
+					ind += ThePaths.m_pathNodes[t->m_nodeIndices[PATH_CAR][0]].group;
 				if(gbShowPedRoadGroups)
-					ind += ThePaths.m_pathNodes[t->m_nodeIndicesPeds[0]].group;
+					ind += ThePaths.m_pathNodes[t->m_nodeIndices[PATH_PED][0]].group;
 				SetAmbientColoursToIndicateRoadGroup(ind);
 			}
 #endif