#include "common.h"
#include "General.h"
#include "FileMgr.h" // only needed for empty function
#include "Camera.h"
#include "Vehicle.h"
#include "World.h"
#include "Lines.h" // for debug
#include "PathFind.h"
//--MIAMI: file done except mobile unused function
bool gbShowPedPaths;
bool gbShowCarPaths;
bool gbShowCarPathsLinks;
CPathFind ThePaths;
#define MAX_DIST INT16_MAX-1
#define MIN_PED_ROUTE_DISTANCE 23.8f
#define NUMTEMPNODES 5000
#define NUMDETACHED_CARS 1024
#define NUMDETACHED_PEDS 1214
#define NUMTEMPEXTERNALNODES 4600
CPathInfoForObject *InfoForTileCars;
CPathInfoForObject *InfoForTilePeds;
CPathInfoForObject *DetachedInfoForTileCars;
CPathInfoForObject *DetachedInfoForTilePeds;
CTempNodeExternal *TempExternalNodes;
int32 NumTempExternalNodes;
int32 NumDetachedPedNodeGroups;
int32 NumDetachedCarNodeGroups;
bool
CPedPath::CalcPedRoute(int8 pathType, CVector position, CVector destination, CVector *pointPoses, int16 *pointsFound, int16 maxPoints)
{
*pointsFound = 0;
CVector vecDistance = destination - position;
if (Abs(vecDistance.x) > MIN_PED_ROUTE_DISTANCE || Abs(vecDistance.y) > MIN_PED_ROUTE_DISTANCE || Abs(vecDistance.z) > MIN_PED_ROUTE_DISTANCE)
return false;
CVector vecPos = (position + destination) * 0.5f;
CVector vecSectorStartPos (vecPos.x - 14.0f, vecPos.y - 14.0f, vecPos.z);
CVector2D vecSectorEndPos (vecPos.x + 28.0f, vecPos.x + 28.0f);
const int16 nodeStartX = (position.x - vecSectorStartPos.x) / 0.7f;
const int16 nodeStartY = (position.y - vecSectorStartPos.y) / 0.7f;
const int16 nodeEndX = (destination.x - vecSectorStartPos.x) / 0.7f;
const int16 nodeEndY = (destination.y - vecSectorStartPos.y) / 0.7f;
if (nodeStartX == nodeEndX && nodeStartY == nodeEndY)
return false;
CPedPathNode pathNodes[40][40];
CPedPathNode pathNodesList[416];
for (int32 x = 0; x < 40; x++) {
for (int32 y = 0; y < 40; y++) {
pathNodes[x][y].bBlockade = false;
pathNodes[x][y].id = INT16_MAX;
pathNodes[x][y].nodeIdX = x;
pathNodes[x][y].nodeIdY = y;
}
}
CWorld::AdvanceCurrentScanCode();
if (pathType != ROUTE_NO_BLOCKADE) {
const int32 nStartX = Max(CWorld::GetSectorIndexX(vecSectorStartPos.x), 0);
const int32 nStartY = Max(CWorld::GetSectorIndexY(vecSectorStartPos.y), 0);
const int32 nEndX = Min(CWorld::GetSectorIndexX(vecSectorEndPos.x), NUMSECTORS_X - 1);
const int32 nEndY = Min(CWorld::GetSectorIndexY(vecSectorEndPos.y), NUMSECTORS_Y - 1);
for (int32 y = nStartY; y <= nEndY; y++) {
for (int32 x = nStartX; x <= nEndX; x++) {
CSector *pSector = CWorld::GetSector(x, y);
AddBlockadeSectorList(pSector->m_lists[ENTITYLIST_VEHICLES], pathNodes, &vecSectorStartPos);
AddBlockadeSectorList(pSector->m_lists[ENTITYLIST_VEHICLES_OVERLAP], pathNodes, &vecSectorStartPos);
AddBlockadeSectorList(pSector->m_lists[ENTITYLIST_OBJECTS], pathNodes, &vecSectorStartPos);
AddBlockadeSectorList(pSector->m_lists[ENTITYLIST_OBJECTS_OVERLAP], pathNodes, &vecSectorStartPos);
}
}
}
for (int32 i = 0; i < 416; i++) {
pathNodesList[i].prev = nil;
pathNodesList[i].next = nil;
}
CPedPathNode *pStartPathNode = &pathNodes[nodeStartX][nodeStartY];
CPedPathNode *pEndPathNode = &pathNodes[nodeEndX][nodeEndY];
pEndPathNode->bBlockade = false;
pEndPathNode->id = 0;
pEndPathNode->prev = nil;
pEndPathNode->next = pathNodesList;
pathNodesList[0].prev = pEndPathNode;
int32 pathNodeIndex = 0;
CPedPathNode *pPreviousNode = nil;
for (; pathNodeIndex < 414; pathNodeIndex++)
{
pPreviousNode = pathNodesList[pathNodeIndex].prev;
while (pPreviousNode && pPreviousNode != pStartPathNode) {
const uint8 nodeIdX = pPreviousNode->nodeIdX;
const uint8 nodeIdY = pPreviousNode->nodeIdY;
if (nodeIdX > 0) {
AddNodeToPathList(&pathNodes[nodeIdX - 1][nodeIdY], pathNodeIndex + 5, pathNodesList);
if (nodeIdY > 0)
AddNodeToPathList(&pathNodes[nodeIdX - 1][nodeIdY - 1], pathNodeIndex + 7, pathNodesList);
if (nodeIdY < 39)
AddNodeToPathList(&pathNodes[nodeIdX - 1][nodeIdY + 1], pathNodeIndex + 7, pathNodesList);
}
if (nodeIdX < 39) {
AddNodeToPathList(&pathNodes[nodeIdX + 1][nodeIdY], pathNodeIndex + 5, pathNodesList);
if (nodeIdY > 0)
AddNodeToPathList(&pathNodes[nodeIdX + 1][nodeIdY - 1], pathNodeIndex + 7, pathNodesList);
if (nodeIdY < 39)
AddNodeToPathList(&pathNodes[nodeIdX + 1][nodeIdY + 1], pathNodeIndex + 7, pathNodesList);
}
if (nodeIdY > 0)
AddNodeToPathList(&pathNodes[nodeIdX][nodeIdY - 1], pathNodeIndex + 5, pathNodesList);
if (nodeIdY < 39)
AddNodeToPathList(&pathNodes[nodeIdX][nodeIdY + 1], pathNodeIndex + 5, pathNodesList);
pPreviousNode = pPreviousNode->prev;
if (!pPreviousNode)
break;
}
if (pPreviousNode && pPreviousNode == pStartPathNode)
break;
}
if (pathNodeIndex == 414)
return false;
CPedPathNode *pPathNode = pStartPathNode;
for (*pointsFound = 0; pPathNode != pEndPathNode && *pointsFound < maxPoints; ++ *pointsFound) {
const uint8 nodeIdX = pPathNode->nodeIdX;
const uint8 nodeIdY = pPathNode->nodeIdY;
if (nodeIdX > 0 && pathNodes[nodeIdX - 1][nodeIdY].id + 5 == pPathNode->id)
pPathNode = &pathNodes[nodeIdX - 1][nodeIdY];
else if (nodeIdX > 39 && pathNodes[nodeIdX + 1][nodeIdY].id + 5 == pPathNode->id)
pPathNode = &pathNodes[nodeIdX + 1][nodeIdY];
else if (nodeIdY > 0 && pathNodes[nodeIdX][nodeIdY - 1].id + 5 == pPathNode->id)
pPathNode = &pathNodes[nodeIdX][nodeIdY - 1];
else if (nodeIdY > 39 && pathNodes[nodeIdX][nodeIdY + 1].id + 5 == pPathNode->id)
pPathNode = &pathNodes[nodeIdX][nodeIdY + 1];
else if (nodeIdX > 0 && nodeIdY > 0 && pathNodes[nodeIdX - 1][nodeIdY - 1].id + 7 == pPathNode->id)
pPathNode = &pathNodes[nodeIdX - 1][nodeIdY - 1];
else if (nodeIdX > 0 && nodeIdY < 39 && pathNodes[nodeIdX - 1][nodeIdY + 1].id + 7 == pPathNode->id)
pPathNode = &pathNodes[nodeIdX - 1][nodeIdY + 1];
else if (nodeIdX < 39 && nodeIdY > 0 && pathNodes[nodeIdX + 1][nodeIdY - 1].id + 7 == pPathNode->id)
pPathNode = &pathNodes[nodeIdX + 1][nodeIdY - 1];
else if (nodeIdX < 39 && nodeIdY < 39 && pathNodes[nodeIdX + 1][nodeIdY + 1].id + 7 == pPathNode->id)
pPathNode = &pathNodes[nodeIdX + 1][nodeIdY + 1];
pointPoses[*pointsFound] = vecSectorStartPos;
pointPoses[*pointsFound].x += pPathNode->nodeIdX * 0.7f;
pointPoses[*pointsFound].y += pPathNode->nodeIdY * 0.7f;
}
return true;
}
void
CPedPath::AddNodeToPathList(CPedPathNode *pNodeToAdd, int16 id, CPedPathNode *pNodeList)
{
if (!pNodeToAdd->bBlockade && id < pNodeToAdd->id) {
if (pNodeToAdd->id != INT16_MAX)
RemoveNodeFromList(pNodeToAdd);
AddNodeToList(pNodeToAdd, id, pNodeList);
}
}
void
CPedPath::RemoveNodeFromList(CPedPathNode *pNode)
{
pNode->next->prev = pNode->prev;
if (pNode->prev)
pNode->prev->next = pNode->next;
}
void
CPedPath::AddNodeToList(CPedPathNode *pNode, int16 index, CPedPathNode *pList)
{
pNode->prev = pList[index].prev;
pNode->next = &pList[index];
if (pList[index].prev)
pList[index].prev->next = pNode;
pList[index].prev = pNode;
pNode->id = index;
}
void
CPedPath::AddBlockadeSectorList(CPtrList& list, CPedPathNode(*pathNodes)[40], CVector *pPosition)
{
CPtrNode* listNode = list.first;
while (listNode) {
CEntity* pEntity = (CEntity*)listNode->item;
if (pEntity->m_scanCode != CWorld::GetCurrentScanCode() && pEntity->bUsesCollision) {
pEntity->m_scanCode = CWorld::GetCurrentScanCode();
AddBlockade(pEntity, pathNodes, pPosition);
}
listNode = listNode->next;
}
}
void
CPedPath::AddBlockade(CEntity *pEntity, CPedPathNode(*pathNodes)[40], CVector *pPosition)
{
const CBox& boundingBox = pEntity->GetColModel()->boundingBox;
const float fBoundMaxY = boundingBox.max.y + 0.3f;
const float fBoundMinY = boundingBox.min.y - 0.3f;
const float fBoundMaxX = boundingBox.max.x + 0.3f;
const float fDistanceX = pPosition->x - pEntity->m_matrix.GetPosition().x;
const float fDistanceY = pPosition->y - pEntity->m_matrix.GetPosition().y;
const float fBoundRadius = pEntity->GetBoundRadius();
CVector vecBoundCentre;
pEntity->GetBoundCentre(vecBoundCentre);
if (vecBoundCentre.x + fBoundRadius >= pPosition->x &&
vecBoundCentre.y + fBoundRadius >= pPosition->y &&
vecBoundCentre.x - fBoundRadius <= pPosition->x + 28.0f &&
vecBoundCentre.y - fBoundRadius <= pPosition->y + 28.0f) {
for (int16 x = 0; x < 40; x++) {
const float pointX = x * 0.7f + fDistanceX;
for (int16 y = 0; y < 40; y++) {
if (!pathNodes[x][y].bBlockade) {
const float pointY = y * 0.7f + fDistanceY;
CVector2D point(pointX, pointY);
if (fBoundMaxX > Abs(DotProduct2D(point, pEntity->m_matrix.GetRight()))) {
float fDotProduct = DotProduct2D(point, pEntity->m_matrix.GetForward());
if (fBoundMaxY > fDotProduct && fBoundMinY < fDotProduct)
pathNodes[x][y].bBlockade = true;
}
}
}
}
}
}
// Make sure all externals link TO an internal
void
CPathInfoForObject::SwapConnectionsToBeRightWayRound(void)
{
int e, i;
CPathInfoForObject *tile = this;
for(e = 0; e < 12; e++)
if(tile[e].type == NodeTypeExtern && tile[e].next < 0)
for(i = 0; i < 12; i++)
if(tile[i].type == NodeTypeIntern && tile[i].next == e){
tile[e].next = i;
tile[i].next = -1;
bool tmp = !!tile[e].crossing;
tile[e].crossing = tile[i].crossing;
tile[i].crossing = tmp;
}
}
void
CPathFind::Init(void)
{
int i;
m_numPathNodes = 0;
m_numMapObjects = 0;
m_numConnections = 0;
m_numCarPathLinks = 0;
unk = 0;
NumTempExternalNodes = 0;
for(i = 0; i < NUM_PATHNODES; i++)
m_pathNodes[i].distance = MAX_DIST;
}
void
CPathFind::AllocatePathFindInfoMem(int16 numPathGroups)
{
delete[] InfoForTileCars;
InfoForTileCars = nil;
delete[] InfoForTilePeds;
InfoForTilePeds = nil;
// NB: MIAMI doesn't use numPathGroups here but hardcodes PATHNODESIZE
InfoForTileCars = new CPathInfoForObject[12*PATHNODESIZE];
memset(InfoForTileCars, 0, 12*PATHNODESIZE*sizeof(CPathInfoForObject));
InfoForTilePeds = new CPathInfoForObject[12*PATHNODESIZE];
memset(InfoForTilePeds, 0, 12*PATHNODESIZE*sizeof(CPathInfoForObject));
delete[] DetachedInfoForTileCars;
DetachedInfoForTileCars = nil;
delete[] DetachedInfoForTilePeds;
DetachedInfoForTilePeds = nil;
DetachedInfoForTileCars = new CPathInfoForObject[12*NUMDETACHED_CARS];
memset(DetachedInfoForTileCars, 0, 12*NUMDETACHED_CARS*sizeof(CPathInfoForObject));
DetachedInfoForTilePeds = new CPathInfoForObject[12*NUMDETACHED_PEDS];
memset(DetachedInfoForTilePeds, 0, 12*NUMDETACHED_PEDS*sizeof(CPathInfoForObject));
TempExternalNodes = new CTempNodeExternal[NUMTEMPEXTERNALNODES];
memset(TempExternalNodes, 0, NUMTEMPEXTERNALNODES*sizeof(CTempNodeExternal));
NumTempExternalNodes = 0;
NumDetachedPedNodeGroups = 0;
NumDetachedCarNodeGroups = 0;
}
void
CPathFind::RegisterMapObject(CTreadable *mapObject)
{
m_mapObjects[m_numMapObjects++] = mapObject;
}
void
CPathFind::StoreNodeInfoPed(int16 id, int16 node, int8 type, int8 next, int16 x, int16 y, int16 z, float width, bool crossing, uint8 spawnRate)
{
int i;
i = id*12 + node;
InfoForTilePeds[i].type = type;
InfoForTilePeds[i].next = next;
InfoForTilePeds[i].x = x/16.0f;
InfoForTilePeds[i].y = y/16.0f;
InfoForTilePeds[i].z = z/16.0f;
InfoForTilePeds[i].width = 8.0f*Min(width, 15.0f);
InfoForTilePeds[i].numLeftLanes = 0;
InfoForTilePeds[i].numRightLanes = 0;
InfoForTilePeds[i].crossing = crossing;
InfoForTilePeds[i].speedLimit = 0;
InfoForTilePeds[i].roadBlock = false;
InfoForTilePeds[i].disabled = false;
InfoForTilePeds[i].waterPath = false;
InfoForTilePeds[i].onlySmallBoats = false;
InfoForTilePeds[i].betweenLevels = false;
InfoForTilePeds[i].spawnRate = Min(spawnRate, 15);
if(node == 11)
InfoForTilePeds[id*12].SwapConnectionsToBeRightWayRound();
}
void
CPathFind::StoreNodeInfoCar(int16 id, int16 node, int8 type, int8 next, int16 x, int16 y, int16 z, float width, int8 numLeft, int8 numRight,
bool disabled, bool betweenLevels, uint8 speedLimit, bool roadBlock, bool waterPath, uint8 spawnRate)
{
int i;
i = id*12 + node;
InfoForTileCars[i].type = type;
InfoForTileCars[i].next = next;
InfoForTileCars[i].x = x/16.0f;
InfoForTileCars[i].y = y/16.0f;
InfoForTileCars[i].z = z/16.0f;
InfoForTilePeds[i].width = 8.0f*Min(width, 15.0f);
InfoForTileCars[i].numLeftLanes = numLeft;
InfoForTileCars[i].numRightLanes = numRight;
InfoForTilePeds[i].crossing = false;
InfoForTilePeds[i].speedLimit = 0;
InfoForTilePeds[i].roadBlock = false;
InfoForTilePeds[i].disabled = false;
InfoForTilePeds[i].waterPath = false;
InfoForTilePeds[i].onlySmallBoats = false;
InfoForTilePeds[i].betweenLevels = false;
InfoForTilePeds[i].spawnRate = Min(spawnRate, 15);
if(node == 11)
InfoForTileCars[id*12].SwapConnectionsToBeRightWayRound();
}
void
CPathFind::StoreDetachedNodeInfoPed(int32 node, int8 type, int32 next, float x, float y, float z, float width, bool crossing,
bool disabled, bool betweenLevels, uint8 spawnRate)
{
int i;
if(NumDetachedPedNodeGroups >= NUMDETACHED_PEDS)
return;
i = NumDetachedPedNodeGroups*12 + node;
DetachedInfoForTilePeds[i].type = type;
DetachedInfoForTilePeds[i].next = next;
DetachedInfoForTilePeds[i].x = x/16.0f;
DetachedInfoForTilePeds[i].y = y/16.0f;
DetachedInfoForTilePeds[i].z = z/16.0f;
DetachedInfoForTilePeds[i].width = 8.0f*Min(width, 31.0f);
DetachedInfoForTilePeds[i].numLeftLanes = 0;
DetachedInfoForTilePeds[i].numRightLanes = 0;
DetachedInfoForTilePeds[i].crossing = crossing;
DetachedInfoForTilePeds[i].speedLimit = 0;
DetachedInfoForTilePeds[i].roadBlock = false;
DetachedInfoForTilePeds[i].disabled = disabled;
DetachedInfoForTilePeds[i].waterPath = false;
DetachedInfoForTilePeds[i].onlySmallBoats = false;
DetachedInfoForTilePeds[i].betweenLevels = betweenLevels;
DetachedInfoForTilePeds[i].spawnRate = Min(spawnRate, 15);
if(node == 11){
DetachedInfoForTilePeds[NumDetachedPedNodeGroups*12].SwapConnectionsToBeRightWayRound();
NumDetachedPedNodeGroups++;
}
}
void
CPathFind::StoreDetachedNodeInfoCar(int32 node, int8 type, int32 next, float x, float y, float z, float width, int8 numLeft, int8 numRight,
bool disabled, bool betweenLevels, uint8 speedLimit, bool roadBlock, bool waterPath, uint8 spawnRate, bool onlySmallBoats)
{
int i;
if(NumDetachedCarNodeGroups >= NUMDETACHED_CARS)
return;
i = NumDetachedCarNodeGroups*12 + node;
DetachedInfoForTileCars[i].type = type;
DetachedInfoForTileCars[i].next = next;
DetachedInfoForTileCars[i].x = x/16.0f;
DetachedInfoForTileCars[i].y = y/16.0f;
DetachedInfoForTileCars[i].z = z/16.0f;
DetachedInfoForTileCars[i].width = 8.0f*Min(width, 15.0f);
DetachedInfoForTileCars[i].numLeftLanes = numLeft;
DetachedInfoForTileCars[i].numRightLanes = numRight;
DetachedInfoForTileCars[i].crossing = false;
DetachedInfoForTileCars[i].speedLimit = speedLimit;
DetachedInfoForTileCars[i].roadBlock = roadBlock;
DetachedInfoForTileCars[i].disabled = disabled;
DetachedInfoForTileCars[i].waterPath = waterPath;
DetachedInfoForTileCars[i].onlySmallBoats = onlySmallBoats;
DetachedInfoForTileCars[i].betweenLevels = betweenLevels;
DetachedInfoForTileCars[i].spawnRate = Min(spawnRate, 15);
if(node == 11){
DetachedInfoForTileCars[NumDetachedCarNodeGroups*12].SwapConnectionsToBeRightWayRound();
NumDetachedCarNodeGroups++;
}
}
void
CPathFind::CalcNodeCoors(float x, float y, float z, int id, CVector *out)
{
CVector pos;
pos.x = x;
pos.y = y;
pos.z = z;
*out = m_mapObjects[id]->GetMatrix() * pos;
}
bool
CPathFind::LoadPathFindData(void)
{
CFileMgr::SetDir("");
return false;
}
void
CPathFind::PreparePathData(void)
{
int i, j;
int numExtern, numIntern;
CTempNode *tempNodes;
printf("PreparePathData\n");
if(!CPathFind::LoadPathFindData() && // empty
InfoForTileCars && InfoForTilePeds &&
DetachedInfoForTileCars && DetachedInfoForTilePeds && TempExternalNodes){
tempNodes = new CTempNode[NUMTEMPNODES];
m_numConnections = 0;
for(i = 0; i < PATHNODESIZE; i++){
numExtern = 0;
numIntern = 0;
for(j = 0; j < 12; j++){
if(InfoForTileCars[i*12 + j].type == NodeTypeExtern)
numExtern++;
if(InfoForTileCars[i*12 + j].type == NodeTypeIntern)
numIntern++;
}
if(numIntern > 1 && numExtern != 2)
printf("ILLEGAL BLOCK. MORE THAN 1 INTERNALS AND NOT 2 EXTERNALS (Modelindex:%d)\n", i);
}
int numExternDetached, numInternDetached;
for(i = 0; i < NUMDETACHED_CARS; i++){
numExternDetached = 0;
numInternDetached = 0;
for(j = 0; j < 12; j++){
if(DetachedInfoForTileCars[i*12 + j].type == NodeTypeExtern)
numExternDetached++;
if(DetachedInfoForTilePeds[i*12 + j].type == NodeTypeIntern)
numInternDetached++;
}
// no diagnostic here
}
for(i = 0; i < PATHNODESIZE; i++)
for(j = 0; j < 12; j++)
if(InfoForTileCars[i*12 + j].type == NodeTypeExtern){
// MIAMI has MI:%d here but no argument for it
if(InfoForTileCars[i*12 + j].numLeftLanes < 0)
printf("ILLEGAL BLOCK. NEGATIVE NUMBER OF LANES (Obj:%d)\n", i);
if(InfoForTileCars[i*12 + j].numRightLanes < 0)
printf("ILLEGAL BLOCK. NEGATIVE NUMBER OF LANES (Obj:%d)\n", i);
if(InfoForTileCars[i*12 + j].numLeftLanes + InfoForTileCars[i*12 + j].numRightLanes <= 0)
printf("ILLEGAL BLOCK. NO LANES IN NODE (Obj:%d)\n", i);
}
for(i = 0; i < NUMDETACHED_CARS; i++)
for(j = 0; j < 12; j++)
if(DetachedInfoForTileCars[i*12 + j].type == NodeTypeExtern){
// MI:%d here but no argument for it
if(DetachedInfoForTileCars[i*12 + j].numLeftLanes < 0)
printf("ILLEGAL BLOCK. NEGATIVE NUMBER OF LANES (Obj:%d)\n", i);
if(DetachedInfoForTileCars[i*12 + j].numRightLanes < 0)
printf("ILLEGAL BLOCK. NEGATIVE NUMBER OF LANES (Obj:%d)\n", i);
if(DetachedInfoForTileCars[i*12 + j].numLeftLanes + DetachedInfoForTileCars[i*12 + j].numRightLanes <= 0)
printf("ILLEGAL BLOCK. NO LANES IN NODE (Obj:%d)\n", i);
}
m_numPathNodes = 0;
PreparePathDataForType(PATH_CAR, tempNodes, InfoForTileCars, 1.0f, DetachedInfoForTileCars, NumDetachedCarNodeGroups);
m_numCarPathNodes = m_numPathNodes;
PreparePathDataForType(PATH_PED, tempNodes, InfoForTilePeds, 1.0f, DetachedInfoForTilePeds, NumDetachedPedNodeGroups);
m_numPedPathNodes = m_numPathNodes - m_numCarPathNodes;
delete[] tempNodes;
CountFloodFillGroups(PATH_CAR);
CountFloodFillGroups(PATH_PED);
delete[] InfoForTileCars;
InfoForTileCars = nil;
delete[] InfoForTilePeds;
InfoForTilePeds = nil;
delete[] DetachedInfoForTileCars;
DetachedInfoForTileCars = nil;
delete[] DetachedInfoForTilePeds;
DetachedInfoForTilePeds = nil;
delete[] TempExternalNodes;
TempExternalNodes = nil;
}
printf("Done with PreparePathData\n");
}
/* String together connected nodes in a list by a flood fill algorithm */
void
CPathFind::CountFloodFillGroups(uint8 type)
{
int start, end;
int i, l;
uint16 n;
CPathNode *node, *prev;
switch(type){
case PATH_CAR:
start = 0;
end = m_numCarPathNodes;
break;
case PATH_PED:
start = m_numCarPathNodes;
end = start + m_numPedPathNodes;
break;
}
for(i = start; i < end; i++)
m_pathNodes[i].group = 0;
n = 0;
for(;;){
n++;
if(n > 1500){
for(i = start; m_pathNodes[i].group && i < end; i++);
printf("NumNodes:%d Accounted for:%d\n", end - start, i - start);
}
// Look for unvisited node
for(i = start; m_pathNodes[i].group && i < end; i++);
if(i == end)
break;
node = &m_pathNodes[i];
node->SetNext(nil);
node->group = n;
if(node->numLinks == 0){
if(type == PATH_CAR)
printf("Single car node: %f %f %f\n",
node->GetX(), node->GetY(), node->GetZ());
else
printf("Single ped node: %f %f %f\n",
node->GetX(), node->GetY(), node->GetZ());
}
while(node){
prev = node;
node = node->GetNext();
for(i = 0; i < prev->numLinks; i++){
l = ConnectedNode(prev->firstLink + i);
if(m_pathNodes[l].group == 0){
m_pathNodes[l].group = n;
if(m_pathNodes[l].group == 0)
m_pathNodes[l].group = INT8_MIN;
m_pathNodes[l].SetNext(node);
node = &m_pathNodes[l];
}
}
}
}
m_numGroups[type] = n-1;
printf("GraphType:%d. FloodFill groups:%d\n", type, n);
}
int32 TempListLength;
void
CPathFind::PreparePathDataForType(uint8 type, CTempNode *tempnodes, CPathInfoForObject *objectpathinfo,
float maxdist, CPathInfoForObject *detachednodes, int numDetached)
{
static CVector CoorsXFormed;
int i, j, k;
int l1, l2;
int start;
float posx, posy;
float dx, dy, mag;
float nearestDist;
int nearestId;
int oldNumPathNodes, oldNumLinks;
float dist;
int iseg, jseg;
int done, cont;
int tileStart;
oldNumPathNodes = m_numPathNodes;
oldNumLinks = m_numConnections;
#define OBJECTINDEX(n) (mapObjIndices[(n)])
int16 *mapObjIndices = new int16[NUM_PATHNODES];
NumTempExternalNodes = 0;
// Calculate internal nodes, store them and connect them to defining object
for(i = 0; i < m_numMapObjects; i++){
tileStart = m_numPathNodes;
start = 12 * m_mapObjects[i]->GetModelIndex();
for(j = 0; j < 12; j++){
if(objectpathinfo[start + j].type == NodeTypeIntern){
CalcNodeCoors(
objectpathinfo[start + j].x,
objectpathinfo[start + j].y,
objectpathinfo[start + j].z,
i,
&CoorsXFormed);
m_pathNodes[m_numPathNodes].SetPosition(CoorsXFormed);
OBJECTINDEX(m_numPathNodes) = i;
m_pathNodes[m_numPathNodes].width = objectpathinfo[start + j].width;
m_pathNodes[m_numPathNodes].speedLimit = objectpathinfo[start + j].speedLimit;
m_pathNodes[m_numPathNodes].spawnRate = objectpathinfo[start + j].spawnRate;
m_pathNodes[m_numPathNodes].bUseInRoadBlock = objectpathinfo[start + j].roadBlock;
m_pathNodes[m_numPathNodes].bDisabled = objectpathinfo[start + j].disabled;
m_pathNodes[m_numPathNodes].bWaterPath = objectpathinfo[start + j].waterPath;
m_pathNodes[m_numPathNodes].bOnlySmallBoats = objectpathinfo[start + j].onlySmallBoats;
m_pathNodes[m_numPathNodes].bBetweenLevels = objectpathinfo[start + j].betweenLevels;
m_numPathNodes++;
}
else if(objectpathinfo[start + j].type == NodeTypeExtern){
CalcNodeCoors(
objectpathinfo[start + j].x,
objectpathinfo[start + j].y,
objectpathinfo[start + j].z,
i,
&CoorsXFormed);
TempExternalNodes[NumTempExternalNodes].pos = CoorsXFormed;
assert(objectpathinfo[start + j].next >= 0);
TempExternalNodes[NumTempExternalNodes].next = tileStart + objectpathinfo[start + j].next;
TempExternalNodes[NumTempExternalNodes].numLeftLanes = objectpathinfo[start + j].numLeftLanes;
TempExternalNodes[NumTempExternalNodes].numRightLanes = objectpathinfo[start + j].numRightLanes;
TempExternalNodes[NumTempExternalNodes].width = objectpathinfo[start + j].width;
TempExternalNodes[NumTempExternalNodes].isCross = !!objectpathinfo[start + j].crossing;
NumTempExternalNodes++;
}
}
}
// Same thing for detached nodes
for(i = 0; i < numDetached; i++){
tileStart = m_numPathNodes;
start = 12*i;
for(j = 0; j < 12; j++){
if(detachednodes[start + j].type == NodeTypeIntern){
CVector pos;
pos.x = detachednodes[start + j].x;
pos.y = detachednodes[start + j].y;
pos.z = detachednodes[start + j].z;
m_pathNodes[m_numPathNodes].SetPosition(pos);
mapObjIndices[m_numPathNodes] = -(i+1);
m_pathNodes[m_numPathNodes].width = detachednodes[start + j].width;
m_pathNodes[m_numPathNodes].speedLimit = detachednodes[start + j].speedLimit;
m_pathNodes[m_numPathNodes].spawnRate = detachednodes[start + j].spawnRate;
m_pathNodes[m_numPathNodes].bUseInRoadBlock = detachednodes[start + j].roadBlock;
m_pathNodes[m_numPathNodes].bDisabled = detachednodes[start + j].disabled;
m_pathNodes[m_numPathNodes].bWaterPath = detachednodes[start + j].waterPath;
m_pathNodes[m_numPathNodes].bOnlySmallBoats = detachednodes[start + j].onlySmallBoats;
m_pathNodes[m_numPathNodes].bBetweenLevels = detachednodes[start + j].betweenLevels;
m_numPathNodes++;
}else if(detachednodes[start + j].type == NodeTypeExtern){
TempExternalNodes[NumTempExternalNodes].pos.x = detachednodes[start + j].x;
TempExternalNodes[NumTempExternalNodes].pos.y = detachednodes[start + j].y;
TempExternalNodes[NumTempExternalNodes].pos.z = detachednodes[start + j].z;
assert(detachednodes[start + j].next >= 0);
TempExternalNodes[NumTempExternalNodes].next = tileStart + detachednodes[start + j].next;
TempExternalNodes[NumTempExternalNodes].numLeftLanes = detachednodes[start + j].numLeftLanes;
TempExternalNodes[NumTempExternalNodes].numRightLanes = detachednodes[start + j].numRightLanes;
TempExternalNodes[NumTempExternalNodes].width = detachednodes[start + j].width;
TempExternalNodes[NumTempExternalNodes].isCross = !!detachednodes[start + j].crossing;
NumTempExternalNodes++;
}
}
}
// Insert external nodes into TempList
TempListLength = 0;
for(i = 0; i < NumTempExternalNodes; i++){
// find closest unconnected node
nearestId = -1;
nearestDist = maxdist;
for(k = 0; k < TempListLength; k++){
if(tempnodes[k].linkState != 1)
continue;
dx = tempnodes[k].pos.x - TempExternalNodes[i].pos.x;
if(Abs(dx) < nearestDist){
dy = tempnodes[k].pos.y - TempExternalNodes[i].pos.y;
if(Abs(dy) < nearestDist){
nearestDist = Max(Abs(dx), Abs(dy));
nearestId = k;
}
}
}
if(nearestId < 0){
// None found, add this one to temp list
tempnodes[TempListLength].pos = TempExternalNodes[i].pos;
// link to connecting internal node
tempnodes[TempListLength].link1 = TempExternalNodes[i].next;
if(type == PATH_CAR){
tempnodes[TempListLength].numLeftLanes = TempExternalNodes[i].numLeftLanes;
tempnodes[TempListLength].numRightLanes = TempExternalNodes[i].numRightLanes;
}
tempnodes[TempListLength].width = TempExternalNodes[i].width;
tempnodes[TempListLength].isCross = TempExternalNodes[i].isCross;
tempnodes[TempListLength++].linkState = 1;
}else{
// Found nearest, connect it to our neighbour
tempnodes[nearestId].link2 = TempExternalNodes[i].next;
tempnodes[nearestId].linkState = 2;
// collapse this node with nearest we found
dx = m_pathNodes[tempnodes[nearestId].link1].GetX() - m_pathNodes[tempnodes[nearestId].link2].GetX();
dy = m_pathNodes[tempnodes[nearestId].link1].GetY() - m_pathNodes[tempnodes[nearestId].link2].GetY();
tempnodes[nearestId].pos = (tempnodes[nearestId].pos + TempExternalNodes[i].pos)*0.5f;
mag = Sqrt(dx*dx + dy*dy);
tempnodes[nearestId].dirX = dx/mag * 100;
tempnodes[nearestId].dirY = dy/mag * 100;
tempnodes[nearestId].width = Max(tempnodes[nearestId].width, TempExternalNodes[i].width);
if(TempExternalNodes[i].isCross)
tempnodes[nearestId].isCross = true; // TODO: is this guaranteed to be false otherwise?
// do something when number of lanes doesn't agree
if(type == PATH_CAR)
if(tempnodes[nearestId].numLeftLanes != 0 && tempnodes[nearestId].numRightLanes != 0 &&
(TempExternalNodes[i].numLeftLanes == 0 || TempExternalNodes[i].numRightLanes == 0)){
// why switch left and right here?
tempnodes[nearestId].numLeftLanes = TempExternalNodes[i].numRightLanes;
tempnodes[nearestId].numRightLanes = TempExternalNodes[i].numLeftLanes;
}
}
}
// Loop through previously added internal nodes and link them
for(i = oldNumPathNodes; i < m_numPathNodes; i++){
// Init link
m_pathNodes[i].numLinks = 0;
m_pathNodes[i].firstLink = m_numConnections;
// See if node connects to external nodes
for(j = 0; j < TempListLength; j++){
if(tempnodes[j].linkState != 2)
continue;
// Add link to other side of the external
// NB this clears the flags in MIAMI
if(tempnodes[j].link1 == i)
m_connections[m_numConnections] = tempnodes[j].link2;
else if(tempnodes[j].link2 == i)
m_connections[m_numConnections] = tempnodes[j].link1;
else
continue;
dist = (m_pathNodes[i].GetPosition() - m_pathNodes[ConnectedNode(m_numConnections)].GetPosition()).Magnitude();
m_distances[m_numConnections] = Min(dist, 255);
if(tempnodes[j].isCross)
m_connections[j] |= 0x8000; // crosses road flag
if(type == PATH_CAR){
// IMPROVE: use a goto here
// Find existing car path link
for(k = 0; k < m_numCarPathLinks; k++){
if(m_carPathLinks[k].dirX == tempnodes[j].dirX &&
m_carPathLinks[k].dirY == tempnodes[j].dirY &&
m_carPathLinks[k].x == (int)(tempnodes[j].pos.x*8.0f) &&
m_carPathLinks[k].y == (int)(tempnodes[j].pos.y*8.0f)){
m_carPathConnections[m_numConnections] = k;
k = m_numCarPathLinks;
}
}
// k is m_numCarPathLinks+1 if we found one
if(k == m_numCarPathLinks){
m_carPathLinks[m_numCarPathLinks].dirX = tempnodes[j].dirX;
m_carPathLinks[m_numCarPathLinks].dirY = tempnodes[j].dirY;
m_carPathLinks[m_numCarPathLinks].x = tempnodes[j].pos.x*8.0f;
m_carPathLinks[m_numCarPathLinks].y = tempnodes[j].pos.y*8.0f;
m_carPathLinks[m_numCarPathLinks].trafficLightDirection = false;
m_carPathLinks[m_numCarPathLinks].width = tempnodes[j].width;
m_carPathLinks[m_numCarPathLinks].pathNodeIndex = i;
m_carPathLinks[m_numCarPathLinks].numLeftLanes = tempnodes[j].numLeftLanes;
m_carPathLinks[m_numCarPathLinks].numRightLanes = tempnodes[j].numRightLanes;
m_carPathLinks[m_numCarPathLinks].trafficLightType = 0;
assert(m_numCarPathLinks <= NUM_CARPATHLINKS);
m_carPathConnections[m_numConnections] = m_numCarPathLinks++;
}
}
m_pathNodes[i].numLinks++;
m_numConnections++;
}
CPathInfoForObject *tile;
if(mapObjIndices[i] < 0){
if(type == PATH_CAR)
tile = &DetachedInfoForTileCars[12 * (-1 - mapObjIndices[i])];
else
tile = &DetachedInfoForTilePeds[12 * (-1 - mapObjIndices[i])];
}else{
if(type == PATH_CAR)
tile = &InfoForTileCars[12 * m_mapObjects[mapObjIndices[i]]->GetModelIndex()];
else
tile = &InfoForTilePeds[12 * m_mapObjects[mapObjIndices[i]]->GetModelIndex()];
}
// Find i inside path segment
iseg = 0;
for(j = Max(oldNumPathNodes, i-12); j < i; j++)
if(OBJECTINDEX(j) == OBJECTINDEX(i))
iseg++;
// Add links to other internal nodes
for(j = Max(oldNumPathNodes, i-12); j < Min(m_numPathNodes, i+12); j++){
if(OBJECTINDEX(i) != OBJECTINDEX(j) || i == j)
continue;
// N.B.: in every path segment, the externals have to be at the end
jseg = j-i + iseg;
if(tile[iseg].next == jseg ||
tile[jseg].next == iseg){
// Found a link between i and jConnectionSetCrossesRoad
// NB this clears the flags in MIAMI
m_connections[m_numConnections] = j;
dist = (m_pathNodes[i].GetPosition() - m_pathNodes[j].GetPosition()).Magnitude();
m_distances[m_numConnections] = Min(dist, 255);
if(type == PATH_CAR){
posx = (m_pathNodes[i].GetX() + m_pathNodes[j].GetX())*0.5f;
posy = (m_pathNodes[i].GetY() + m_pathNodes[j].GetY())*0.5f;
dx = m_pathNodes[j].GetX() - m_pathNodes[i].GetX();
dy = m_pathNodes[j].GetY() - m_pathNodes[i].GetY();
mag = Sqrt(dx*dx + dy*dy);
dx /= mag;
dy /= mag;
int width = Max(m_pathNodes[i].width, m_pathNodes[j].width);
if(i < j){
dx = -dx;
dy = -dy;
}
// IMPROVE: use a goto here
// Find existing car path link
for(k = 0; k < m_numCarPathLinks; k++){
if(m_carPathLinks[k].dirX == (int)(dx*100.0f) &&
m_carPathLinks[k].dirY == (int)(dy*100.0f) &&
m_carPathLinks[k].x == (int)(posx*8.0f) &&
m_carPathLinks[k].y == (int)(posy*8.0f)){
m_carPathConnections[m_numConnections] = k;
k = m_numCarPathLinks;
}
}
// k is m_numCarPathLinks+1 if we found one
if(k == m_numCarPathLinks){
m_carPathLinks[m_numCarPathLinks].dirX = dx*100.0f;
m_carPathLinks[m_numCarPathLinks].dirY = dy*100.0f;
m_carPathLinks[m_numCarPathLinks].x = posx*8.0f;
m_carPathLinks[m_numCarPathLinks].y = posy*8.0f;
m_carPathLinks[m_numCarPathLinks].trafficLightDirection = false;
m_carPathLinks[m_numCarPathLinks].width = width;
m_carPathLinks[m_numCarPathLinks].pathNodeIndex = i;
m_carPathLinks[m_numCarPathLinks].numLeftLanes = -1;
m_carPathLinks[m_numCarPathLinks].numRightLanes = -1;
m_carPathLinks[m_numCarPathLinks].trafficLightType = 0;
assert(m_numCarPathLinks <= NUM_CARPATHLINKS);
m_carPathConnections[m_numConnections] = m_numCarPathLinks++;
}
}else{
// Crosses road
if(tile[iseg].next == jseg && tile[iseg].crossing ||
tile[jseg].next == iseg && tile[jseg].crossing)
m_connections[m_numConnections] |= 0x8000; // crosses road flag
}
m_pathNodes[i].numLinks++;
m_numConnections++;
}
}
}
if(type == PATH_CAR){
done = 0;
// Set number of lanes for all nodes somehow
// very strange code
for(k = 0; !done && k < 12; k++){
done = 1;
for(i = 0; i < m_numPathNodes; i++){
if(m_pathNodes[i].numLinks != 2)
continue;
l1 = m_carPathConnections[m_pathNodes[i].firstLink];
l2 = m_carPathConnections[m_pathNodes[i].firstLink+1];
int8 l1Left = m_carPathLinks[l1].numLeftLanes;
int8 l1Right = m_carPathLinks[l1].numRightLanes;
int8 l2Left = m_carPathLinks[l2].numLeftLanes;
int8 l2Right = m_carPathLinks[l2].numRightLanes;
int8 *l1Leftp, *l1Rightp;
int8 *l2Leftp, *l2Rightp;
if(m_carPathLinks[l1].pathNodeIndex == i){
l1Leftp = &l1Left;
l1Rightp = &l1Right;
}else{
l1Leftp = &l1Right;
l1Rightp = &l1Left;
}
if(m_carPathLinks[l2].pathNodeIndex == i){
l2Leftp = &l2Left;
l2Rightp = &l2Right;
}else{
l2Leftp = &l2Right;
l2Rightp = &l2Left;
}
if(*l1Leftp == -1 && *l2Rightp != -1){
*l1Leftp = *l2Rightp;
done = 0;
}
if(*l1Rightp == -1 && *l2Leftp != -1){
*l1Rightp = *l2Leftp;
done = 0;
}
if(*l2Leftp == -1 && *l1Rightp != -1){
*l2Leftp = *l1Rightp;
done = 0;
}
if(*l2Rightp == -1 && *l1Leftp != -1){
*l2Rightp = *l1Leftp;
done = 0;
}
if(*l1Leftp == -1 && *l2Rightp == -1)
done = 0;
if(*l2Leftp == -1 && *l1Rightp == -1)
done = 0;
m_carPathLinks[l1].numLeftLanes = l1Left;
m_carPathLinks[l1].numRightLanes = l1Right;
m_carPathLinks[l2].numLeftLanes = l2Left;
m_carPathLinks[l2].numRightLanes = l2Right;
}
}
// Fall back to default values for number of lanes
for(i = 0; i < m_numPathNodes; i++)
for(j = 0; j < m_pathNodes[i].numLinks; j++){
k = m_carPathConnections[m_pathNodes[i].firstLink + j];
if(m_carPathLinks[k].numLeftLanes == -1)
m_carPathLinks[k].numLeftLanes = 0;
if(m_carPathLinks[k].numRightLanes == -1)
m_carPathLinks[k].numRightLanes = 0;
}
}
// Set flags for car nodes
if(type == PATH_CAR){
do{
cont = 0;
for(i = 0; i < m_numPathNodes; i++){
// See if node is a dead end, if so, we're not done yet
if(!m_pathNodes[i].bDeadEnd){
k = 0;
for(j = 0; j < m_pathNodes[i].numLinks; j++)
if(!m_pathNodes[ConnectedNode(m_pathNodes[i].firstLink + j)].bDeadEnd)
k++;
if(k < 2){
m_pathNodes[i].bDeadEnd = true;
cont = 1;
}
}
}
}while(cont);
}
// Remove isolated ped nodes
if(type == PATH_PED)
for(i = oldNumPathNodes; i < m_numPathNodes; i++){
if(m_pathNodes[i].numLinks != 0)
continue;
// Remove node
for(j = i; j < m_numPathNodes-1; j++)
m_pathNodes[j] = m_pathNodes[j+1];
// Fix links
for(j = oldNumLinks; j < m_numConnections; j++){
int node = ConnectedNode(j);
if(node >= i)
m_connections[j] = node-1;
}
i--;
m_numPathNodes--;
}
delete[] mapObjIndices;
}
float
CPathFind::CalcRoadDensity(float x, float y)
{
int i, j;
float density = 0.0f;
for(i = 0; i < m_numCarPathNodes; i++){
if(Abs(m_pathNodes[i].GetX() - x) < 80.0f &&
Abs(m_pathNodes[i].GetY() - y) < 80.0f &&
m_pathNodes[i].numLinks > 0){
for(j = 0; j < m_pathNodes[i].numLinks; j++){
int next = ConnectedNode(m_pathNodes[i].firstLink + j);
float dist = (m_pathNodes[i].GetPosition() - m_pathNodes[next].GetPosition()).Magnitude2D();
next = m_carPathConnections[m_pathNodes[i].firstLink + j];
density += m_carPathLinks[next].numLeftLanes * dist;
density += m_carPathLinks[next].numRightLanes * dist;
}
}
}
return density/2500.0f;
}
bool
CPathFind::TestForPedTrafficLight(CPathNode *n1, CPathNode *n2)
{
int i;
for(i = 0; i < n1->numLinks; i++)
if(&m_pathNodes[ConnectedNode(n1->firstLink + i)] == n2)
return ConnectionHasTrafficLight(n1->firstLink + i);
return false;
}
bool
CPathFind::TestCrossesRoad(CPathNode *n1, CPathNode *n2)
{
int i;
for(i = 0; i < n1->numLinks; i++)
if(&m_pathNodes[ConnectedNode(n1->firstLink + i)] == n2)
return ConnectionCrossesRoad(n1->firstLink + i);
return false;
}
void
CPathFind::AddNodeToList(CPathNode *node, int32 listId)
{
int i = listId & 0x1FF;
node->SetNext(m_searchNodes[i].GetNext());
node->SetPrev(&m_searchNodes[i]);
if(m_searchNodes[i].GetNext())
m_searchNodes[i].GetNext()->SetPrev(node);
m_searchNodes[i].SetNext(node);
node->distance = listId;
}
void
CPathFind::RemoveNodeFromList(CPathNode *node)
{
node->GetPrev()->SetNext(node->GetNext());
if(node->GetNext())
node->GetNext()->SetPrev(node->GetPrev());
}
void
CPathFind::RemoveBadStartNode(CVector pos, CPathNode **nodes, int16 *n)
{
int i;
if(*n < 2)
return;
if(DotProduct2D(nodes[1]->GetPosition() - pos, nodes[0]->GetPosition() - pos) < 0.0f){
(*n)--;
for(i = 0; i < *n; i++)
nodes[i] = nodes[i+1];
}
}
#ifdef GTA_BRIDGE
void
CPathFind::SetLinksBridgeLights(float x1, float x2, float y1, float y2, bool enable)
{
int i;
for(i = 0; i < m_numCarPathLinks; i++){
CVector2D pos = m_carPathLinks[i].GetPosition();
if(x1 < pos.x && pos.x < x2 &&
y1 < pos.y && pos.y < y2)
m_carPathLinks[i].bBridgeLights = enable;
}
}
#endif
void
CPathFind::SwitchOffNodeAndNeighbours(int32 nodeId, bool disable)
{
int i, next;
m_pathNodes[nodeId].bDisabled = disable;
if(m_pathNodes[nodeId].numLinks < 3)
for(i = 0; i < m_pathNodes[nodeId].numLinks; i++){
next = ConnectedNode(m_pathNodes[nodeId].firstLink + i);
if(m_pathNodes[next].bDisabled != disable &&
m_pathNodes[next].numLinks < 3)
SwitchOffNodeAndNeighbours(next, disable);
}
}
void
CPathFind::SwitchRoadsOffInArea(float x1, float x2, float y1, float y2, float z1, float z2, bool disable)
{
int i;
for(i = 0; i < m_numCarPathNodes; i++){
CVector pos = m_pathNodes[i].GetPosition();
if(x1 <= pos.x && pos.x <= x2 &&
y1 <= pos.y && pos.y <= y2 &&
z1 <= pos.z && pos.z <= z2 &&
disable != m_pathNodes[i].bDisabled)
SwitchOffNodeAndNeighbours(i, disable);
}
}
void
CPathFind::SwitchPedRoadsOffInArea(float x1, float x2, float y1, float y2, float z1, float z2, bool disable)
{
int i;
for(i = m_numCarPathNodes; i < m_numPathNodes; i++){
CVector pos = m_pathNodes[i].GetPosition();
if(x1 <= pos.x && pos.x <= x2 &&
y1 <= pos.y && pos.y <= y2 &&
z1 <= pos.z && pos.z <= z2 &&
disable != m_pathNodes[i].bDisabled)
SwitchOffNodeAndNeighbours(i, disable);
}
}
void
CPathFind::SwitchRoadsInAngledArea(float x1, float y1, float z1, float x2, float y2, float z2, float length, uint8 type, uint8 mode)
{
int i;
int firstNode, lastNode;
// this is NOT PATH_CAR
if(type != 0){
firstNode = 0;
lastNode = m_numCarPathNodes;
}else{
firstNode = m_numCarPathNodes;
lastNode = m_numPathNodes;
}
if(z1 > z2){
float tmp = z2;
z2 = z1;
z1 = tmp;
}
// angle of vector from p2 to p1
float angle = CGeneral::GetRadianAngleBetweenPoints(x1, y1, x2, y2) + HALFPI;
while(angle < 0.0f) angle += TWOPI;
while(angle > TWOPI) angle -= TWOPI;
// vector from p1 to p2
CVector2D v12(x2 - x1, y2 - y1);
float len12 = v12.Magnitude();
v12 /= len12;
// vector from p2 to new point p3
CVector2D v23(Sin(angle)*length, -(Cos(angle)*length));
v23 /= v23.Magnitude(); // obivously just 'length' but whatever
bool disable = mode == SWITCH_OFF;
for(i = firstNode; i < lastNode; i++){
CVector pos = m_pathNodes[i].GetPosition();
if(pos.z < z1 || pos.z > z2)
continue;
CVector2D d(pos.x - x1, pos.y - y1);
float dot = DotProduct2D(d, v12);
if(dot < 0.0f || dot > len12)
continue;
dot = DotProduct2D(d, v23);
if(dot < 0.0f || dot > length)
continue;
if(m_pathNodes[i].bDisabled != disable)
SwitchOffNodeAndNeighbours(i, disable);
}
}
void
CPathFind::MarkRoadsBetweenLevelsNodeAndNeighbours(int32 nodeId)
{
int i, next;
m_pathNodes[nodeId].bBetweenLevels = true;
if(m_pathNodes[nodeId].numLinks < 3)
for(i = 0; i < m_pathNodes[nodeId].numLinks; i++){
next = ConnectedNode(m_pathNodes[nodeId].firstLink + i);
if(!m_pathNodes[next].bBetweenLevels &&
m_pathNodes[next].numLinks < 3)
MarkRoadsBetweenLevelsNodeAndNeighbours(next);
}
}
void
CPathFind::MarkRoadsBetweenLevelsInArea(float x1, float x2, float y1, float y2, float z1, float z2)
{
int i;
for(i = 0; i < m_numPathNodes; i++){
CVector pos = m_pathNodes[i].GetPosition();
if(x1 < pos.x && pos.x < x2 &&
y1 < pos.y && pos.y < y2 &&
z1 < pos.z && pos.z < z2)
MarkRoadsBetweenLevelsNodeAndNeighbours(i);
}
}
void
CPathFind::PedMarkRoadsBetweenLevelsInArea(float x1, float x2, float y1, float y2, float z1, float z2)
{
int i;
for(i = m_numCarPathNodes; i < m_numPathNodes; i++){
CVector pos = m_pathNodes[i].GetPosition();
if(x1 < pos.x && pos.x < x2 &&
y1 < pos.y && pos.y < y2 &&
z1 < pos.z && pos.z < z2)
MarkRoadsBetweenLevelsNodeAndNeighbours(i);
}
}
int32
CPathFind::FindNodeClosestToCoors(CVector coors, uint8 type, float distLimit, bool ignoreDisabled, bool ignoreBetweenLevels, bool ignoreSelected, bool bWaterPath)
{
int i;
int firstNode, lastNode;
float dist;
float closestDist = 10000.0f;
int closestNode = 0;
switch(type){
case PATH_CAR:
firstNode = 0;
lastNode = m_numCarPathNodes;
break;
case PATH_PED:
firstNode = m_numCarPathNodes;
lastNode = m_numPathNodes;
break;
}
for(i = firstNode; i < lastNode; i++){
if(ignoreDisabled && m_pathNodes[i].bDisabled) continue;
if(ignoreBetweenLevels && m_pathNodes[i].bBetweenLevels) continue;
if(ignoreSelected && m_pathNodes[i].bSelected) continue;
if(bWaterPath != m_pathNodes[i].bWaterPath) continue;
dist = Abs(m_pathNodes[i].GetX() - coors.x) +
Abs(m_pathNodes[i].GetY() - coors.y) +
3.0f*Abs(m_pathNodes[i].GetZ() - coors.z);
if(dist < closestDist){
closestDist = dist;
closestNode = i;
}
}
return closestDist < distLimit ? closestNode : -1;
}
int32
CPathFind::FindNodeClosestToCoorsFavourDirection(CVector coors, uint8 type, float dirX, float dirY)
{
int i;
int firstNode, lastNode;
float dist, dX, dY;
NormalizeXY(dirX, dirY);
float closestDist = 10000.0f;
int closestNode = 0;
switch(type){
case PATH_CAR:
firstNode = 0;
lastNode = m_numCarPathNodes;
break;
case PATH_PED:
firstNode = m_numCarPathNodes;
lastNode = m_numPathNodes;
break;
}
for(i = firstNode; i < lastNode; i++){
dX = m_pathNodes[i].GetX() - coors.x;
dY = m_pathNodes[i].GetY() - coors.y;
dist = Abs(dX) + Abs(dY) +
3.0f*Abs(m_pathNodes[i].GetZ() - coors.z);
if(dist < closestDist){
NormalizeXY(dX, dY);
dist -= (dX*dirX + dY*dirY - 1.0f)*20.0f;
if(dist < closestDist){
closestDist = dist;
closestNode = i;
}
}
}
return closestNode;
}
void
CPathFind::FindNodePairClosestToCoors(CVector coors, uint8 type, int* node1, int* node2, float* angle, float minDist, float maxDist, bool ignoreDisabled, bool ignoreBetweenLevels, bool bWaterPath)
{
int i, j;
int firstNode, lastNode, connectedNode;
float dist;
float closestDist = 10000.0f;
int closestNode = 0, closestConnectedNode = 0;
switch (type) {
case PATH_CAR:
firstNode = 0;
lastNode = m_numCarPathNodes;
break;
case PATH_PED:
firstNode = m_numCarPathNodes;
lastNode = m_numPathNodes;
break;
}
for (i = firstNode; i < lastNode; i++) {
if (ignoreDisabled && m_pathNodes[i].bDisabled) continue;
if (ignoreBetweenLevels && m_pathNodes[i].bBetweenLevels) continue;
if (bWaterPath != m_pathNodes[i].bWaterPath) continue;
dist = Abs(m_pathNodes[i].GetX() - coors.x) +
Abs(m_pathNodes[i].GetY() - coors.y) +
3.0f * Abs(m_pathNodes[i].GetZ() - coors.z);
if (dist < closestDist) {
for (j = 0; j < m_pathNodes[i].numLinks; j++) {
connectedNode = ConnectedNode(m_pathNodes[i].firstLink + j);
if (ignoreDisabled && m_pathNodes[connectedNode].bDisabled) continue;
if (ignoreBetweenLevels && m_pathNodes[connectedNode].bBetweenLevels) continue;
if (bWaterPath != m_pathNodes[connectedNode].bWaterPath) continue;
if ((m_pathNodes[connectedNode].GetPosition() - m_pathNodes[i].GetPosition()).Magnitude() > minDist) {
closestDist = dist;
closestNode = i;
closestConnectedNode = connectedNode;
}
}
}
}
if (closestDist < maxDist) {
*node1 = closestNode;
*node2 = closestConnectedNode;
CVector dir(m_pathNodes[*node2].GetX() - m_pathNodes[*node1].GetX(), m_pathNodes[*node2].GetY() - m_pathNodes[*node1].GetY(), 0.0f);
dir.Normalise();
*angle = RADTODEG(Atan2(-dir.x, dir.y));
}
else {
*node1 = -1;
*node2 = -1;
*angle = 0.0f;
}
}
int32
CPathFind::FindNthNodeClosestToCoors(CVector coors, uint8 type, float distLimit, bool ignoreDisabled, bool ignoreBetweenLevels, int N, bool bWaterPath)
{
int i;
int firstNode, lastNode;
switch (type) {
case PATH_CAR:
firstNode = 0;
lastNode = m_numCarPathNodes;
break;
case PATH_PED:
firstNode = m_numCarPathNodes;
lastNode = m_numPathNodes;
break;
}
for (i = firstNode; i < lastNode; i++)
m_pathNodes[i].bSelected = false;
for (; N > 0; N--) {
i = FindNodeClosestToCoors(coors, type, distLimit, ignoreDisabled, ignoreBetweenLevels, true, bWaterPath);
if (i < 0)
return -1;
m_pathNodes[i].bSelected = true;
}
return FindNodeClosestToCoors(coors, type, distLimit, ignoreDisabled, ignoreBetweenLevels, true, bWaterPath);
}
CVector
CPathFind::FindNodeCoorsForScript(int32 id)
{
// the point is to return valid position in case there is a divider in the middle of the road
if (!m_pathNodes[id].HasDivider() || m_pathNodes[id].numLinks == 0)
return m_pathNodes[id].GetPosition();
CVector2D dir(m_pathNodes[ConnectedNode(m_pathNodes[id].firstLink)].GetX() - m_pathNodes[id].GetX(),
m_pathNodes[ConnectedNode(m_pathNodes[id].firstLink)].GetY() - m_pathNodes[id].GetY());
dir.Normalise();
if (dir.x < 0)
dir = -dir;
return m_pathNodes[id].GetPosition() + CVector(-dir.y, dir.z, 0.0f) * (LANE_WIDTH / 2 + m_pathNodes[id].GetDividerWidth());
}
float
CPathFind::FindNodeOrientationForCarPlacement(int32 nodeId)
{
if(m_pathNodes[nodeId].numLinks == 0)
return 0.0f;
CVector dir = m_pathNodes[ConnectedNode(m_pathNodes[nodeId].firstLink)].GetPosition() - m_pathNodes[nodeId].GetPosition();
dir.z = 0.0f;
dir.Normalise();
return RADTODEG(dir.Heading());
}
float
CPathFind::FindNodeOrientationForCarPlacementFacingDestination(int32 nodeId, float x, float y, bool towards)
{
int i;
CVector targetDir(x - m_pathNodes[nodeId].GetX(), y - m_pathNodes[nodeId].GetY(), 0.0f);
targetDir.Normalise();
CVector dir;
if(m_pathNodes[nodeId].numLinks == 0)
return 0.0f;
int bestNode = ConnectedNode(m_pathNodes[nodeId].firstLink);
#ifdef FIX_BUGS
float bestDot = towards ? -2.0f : 2.0f;
#else
int bestDot = towards ? -2 : 2; // why int?
#endif
for(i = 0; i < m_pathNodes[nodeId].numLinks; i++){
dir = m_pathNodes[ConnectedNode(m_pathNodes[nodeId].firstLink + i)].GetPosition() - m_pathNodes[nodeId].GetPosition();
dir.z = 0.0f;
dir.Normalise();
float angle = DotProduct2D(dir, targetDir);
if(towards){
if(angle > bestDot){
bestDot = angle;
bestNode = ConnectedNode(m_pathNodes[nodeId].firstLink + i);
}
}else{
if(angle < bestDot){
bestDot = angle;
bestNode = ConnectedNode(m_pathNodes[nodeId].firstLink + i);
}
}
}
dir = m_pathNodes[bestNode].GetPosition() - m_pathNodes[nodeId].GetPosition();
dir.z = 0.0f;
dir.Normalise();
return RADTODEG(dir.Heading());
}
bool
CPathFind::GenerateCarCreationCoors(float x, float y, float dirX, float dirY, float spawnDist, float angleLimit, bool forward, CVector *pPosition, int32 *pNode1, int32 *pNode2, float *pPositionBetweenNodes, bool ignoreDisabled)
{
int i, j;
int node1, node2;
float dist1, dist2, d1, d2;
if(m_numCarPathNodes == 0)
return false;
for(i = 0; i < 500; i++){
node1 = (CGeneral::GetRandomNumber()>>3) % m_numCarPathNodes;
if(m_pathNodes[node1].bDisabled && !ignoreDisabled)
continue;
dist1 = Distance2D(m_pathNodes[node1].GetPosition(), x, y);
if(dist1 < Max(spawnDist + 70.0f, spawnDist * 1.7f)){
d1 = m_pathNodes[node1].bWaterPath ? (dist1 - spawnDist * 1.5f) : (dist1 - spawnDist);
for(j = 0; j < m_pathNodes[node1].numLinks; j++){
node2 = ConnectedNode(m_pathNodes[node1].firstLink + j);
if(m_pathNodes[node2].bDisabled && !ignoreDisabled)
continue;
dist2 = Distance2D(m_pathNodes[node2].GetPosition(), x, y);
d2 = m_pathNodes[node2].bWaterPath ? (dist2 - spawnDist * 1.5f) : (dist2 - spawnDist);
if(d1*d2 < 0.0f){
// nodes are on different sides of spawn distance
float f2 = Abs(d1)/(Abs(d1) + Abs(d2));
float f1 = 1.0f - f2;
*pPositionBetweenNodes = f2;
CVector pos = m_pathNodes[node1].GetPosition()*f1 + m_pathNodes[node2].GetPosition()*f2;
CVector2D dist2d(pos.x - x, pos.y - y);
dist2d.Normalise(); // done manually in the game
float dot = DotProduct2D(dist2d, CVector2D(dirX, dirY));
if(forward){
if(dot > angleLimit){
*pNode1 = node1;
*pNode2 = node2;
*pPosition = pos;
return true;
}
}else{
if(dot <= angleLimit){
*pNode1 = node1;
*pNode2 = node2;
*pPosition = pos;
return true;
}
}
}
}
}
}
return false;
}
bool
CPathFind::GeneratePedCreationCoors(float x, float y, float minDist, float maxDist, float minDistOffScreen, float maxDistOffScreen, CVector *pPosition, int32 *pNode1, int32 *pNode2, float *pPositionBetweenNodes, CMatrix *camMatrix)
{
int i;
int node1, node2;
float node1_dist, node2_dist;
static int32 node_cnt;
if(m_numPedPathNodes == 0)
return false;
for(i = 0; i < 230; i++){
if (node_cnt++ >= m_numPedPathNodes)
node_cnt = 0;
node1 = node_cnt + m_numCarPathNodes;
node1_dist = Distance2D(m_pathNodes[node1].GetPosition(), x, y);
if(node1_dist < maxDist+30.0f){
if(m_pathNodes[node1].numLinks != 0)
break;
}
}
if (i >= 230)
return false;
for(i = 0; i < m_pathNodes[node1].numLinks; i++){
int link = m_pathNodes[node1].firstLink + i;
if(ConnectionCrossesRoad(link))
continue;
node2 = ConnectedNode(link);
if(m_pathNodes[node1].bDisabled || m_pathNodes[node2].bDisabled)
continue;
node2_dist = Distance2D(m_pathNodes[node2].GetPosition(), x, y);
if ((node1_dist < maxDist || node2_dist < maxDist) && (node1_dist > minDistOffScreen || node2_dist > minDistOffScreen))
break;
}
if(i >= m_pathNodes[node1].numLinks)
return false;
for(i = 0; i < 5; i++){
float f2 = (CGeneral::GetRandomNumber()&0xFF)/256.0f;
float f1 = 1.0f - f2;
*pPositionBetweenNodes = f2;
CVector pos = m_pathNodes[node1].GetPosition()*f1 + m_pathNodes[node2].GetPosition()*f2;
if(Distance2D(pos, x, y) < maxDist+20.0f){
pos.x += ((CGeneral::GetRandomNumber()&0xFF)-128)*0.01f;
pos.y += ((CGeneral::GetRandomNumber()&0xFF)-128)*0.01f;
float dist = Distance2D(pos, x, y);
bool visible;
if(camMatrix)
visible = TheCamera.IsSphereVisible(pos, 2.0f, camMatrix);
else
visible = TheCamera.IsSphereVisible(pos, 2.0f);
if(!visible){
minDist = minDistOffScreen;
maxDist = maxDistOffScreen;
}
if(visible && (minDist < dist && dist < maxDist) ||
!visible && (minDistOffScreen < dist && dist < maxDistOffScreen)){
*pNode1 = node1;
*pNode2 = node2;
*pPosition = pos;
bool found;
float groundZ = CWorld::FindGroundZFor3DCoord(pos.x, pos.y, pos.z+2.0f, &found);
if(!found)
return false;
if(Abs(groundZ - pos.z) > 3.0f)
return false;
pPosition->z = groundZ;
return true;
}
}
}
return false;
}
void
CPathFind::FindNextNodeWandering(uint8 type, CVector coors, CPathNode **lastNode, CPathNode **nextNode, uint8 curDir, uint8 *nextDir)
{
int i;
CPathNode *node;
if(lastNode == nil || (node = *lastNode) == nil || (coors - (*lastNode)->GetPosition()).MagnitudeSqr() > 7.0f){
int32 nodeIdx = FindNodeClosestToCoors(coors, type, 999999.88f);
node = &m_pathNodes[nodeIdx];
}
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 = ConnectedNode(node->firstLink+i);
if(!node->bDisabled && m_pathNodes[next].bDisabled)
continue;
CVector pedCoors = coors;
pedCoors.z += 1.0f;
CVector nodeCoors = m_pathNodes[next].GetPosition();
nodeCoors.z += 1.0f;
if(!CWorld::GetIsLineOfSightClear(pedCoors, nodeCoors, true, false, false, false, false, false))
continue;
CVector2D nodeDir = m_pathNodes[next].GetPosition() - node->GetPosition();
nodeDir.Normalise();
float dot = DotProduct2D(nodeDir, vCurDir);
if(dot >= bestDot){
*nextNode = &m_pathNodes[next];
bestDot = dot;
// direction is 0, 2, 4, 6 for north, east, south, west
// this could be done simpler...
if(nodeDir.x < 0.0f){
if(2.0f*Abs(nodeDir.y) < -nodeDir.x)
*nextDir = 6; // west
else if(-2.0f*nodeDir.x < nodeDir.y)
*nextDir = 0; // north
else if(2.0f*nodeDir.x > nodeDir.y)
*nextDir = 4; // south
else if(nodeDir.y > 0.0f)
*nextDir = 7; // north west
else
*nextDir = 5; // south west`
}else{
if(2.0f*Abs(nodeDir.y) < nodeDir.x)
*nextDir = 2; // east
else if(2.0f*nodeDir.x < nodeDir.y)
*nextDir = 0; // north
else if(-2.0f*nodeDir.x > nodeDir.y)
*nextDir = 4; // south
else if(nodeDir.y > 0.0f)
*nextDir = 1; // north east
else
*nextDir = 3; // south east`
}
}
}
if(*nextNode == nil){
*nextDir = 0;
*nextNode = node;
}
}
static CPathNode *apNodesToBeCleared[6525];
void
CPathFind::DoPathSearch(uint8 type, CVector start, int32 startNodeId, CVector target, CPathNode **nodes, int16 *pNumNodes, int16 maxNumNodes, CVehicle *vehicle, float *pDist, float distLimit, int32 targetNodeId)
{
int i, j;
// Find target
if(targetNodeId < 0)
targetNodeId = FindNodeClosestToCoors(target, type, distLimit);
if(targetNodeId < 0) {
*pNumNodes = 0;
if(pDist) *pDist = 100000.0f;
return;
}
// Find start
if(startNodeId < 0)
startNodeId = FindNodeClosestToCoors(start, type, 999999.88f);
if(startNodeId < 0) {
*pNumNodes = 0;
if(pDist) *pDist = 100000.0f;
return;
}
if(startNodeId == targetNodeId){
*pNumNodes = 0;
if(pDist) *pDist = 0.0f;
return;
}
if(m_pathNodes[startNodeId].group != m_pathNodes[targetNodeId].group) {
*pNumNodes = 0;
if(pDist) *pDist = 100000.0f;
return;
}
for(i = 0; i < ARRAY_SIZE(m_searchNodes); i++)
m_searchNodes[i].SetNext(nil);
AddNodeToList(&m_pathNodes[targetNodeId], 0);
int numNodesToBeCleared = 0;
apNodesToBeCleared[numNodesToBeCleared++] = &m_pathNodes[targetNodeId];
// Dijkstra's algorithm
// Find distances
int numPathsFound = 0;
for(i = 0; numPathsFound == 0; i = (i+1) & 0x1FF){
CPathNode *node;
for(node = m_searchNodes[i].GetNext(); node; node = node->GetNext()){
if(node == &m_pathNodes[startNodeId])
numPathsFound = 1;
for(j = 0; j < node->numLinks; j++){
int next = ConnectedNode(node->firstLink + j);
int dist = node->distance + m_distances[node->firstLink + j];
if(dist < m_pathNodes[next].distance){
if(m_pathNodes[next].distance != MAX_DIST)
RemoveNodeFromList(&m_pathNodes[next]);
if(m_pathNodes[next].distance == MAX_DIST)
apNodesToBeCleared[numNodesToBeCleared++] = &m_pathNodes[next];
AddNodeToList(&m_pathNodes[next], dist);
}
}
RemoveNodeFromList(node);
}
}
// Find out whence to start tracing back
CPathNode *curNode;
curNode = &m_pathNodes[startNodeId];
*pNumNodes = 0;
if(pDist)
*pDist = m_pathNodes[startNodeId].distance;
nodes[(*pNumNodes)++] = curNode;
// Trace back to target and update list of nodes
while(*pNumNodes < maxNumNodes && curNode != &m_pathNodes[targetNodeId])
for(i = 0; i < curNode->numLinks; i++){
int next = ConnectedNode(curNode->firstLink + i);
if(curNode->distance - m_distances[curNode->firstLink + i] == m_pathNodes[next].distance){
curNode = &m_pathNodes[next];
nodes[(*pNumNodes)++] = curNode;
i = 29030; // could have used a break...
}
}
for(i = 0; i < numNodesToBeCleared; i++)
apNodesToBeCleared[i]->distance = MAX_DIST;
}
static CPathNode *pNodeList[32];
static int16 DummyResult;
static int16 DummyResult2;
bool
CPathFind::TestCoorsCloseness(CVector target, uint8 type, CVector start)
{
float dist;
if(type == PATH_CAR)
DoPathSearch(type, start, -1, target, pNodeList, &DummyResult, 32, nil, &dist, 999999.88f, -1);
else
DoPathSearch(type, start, -1, target, nil, &DummyResult2, 0, nil, &dist, 50.0f, -1);
if(type == PATH_CAR)
return dist < 150.0f;
else
return dist < 100.0f;
}
void
CPathFind::Save(uint8 *buf, uint32 *size)
{
int i;
int n = m_numPathNodes/8 + 1;
*size = 2*n;
for(i = 0; i < m_numPathNodes; i++)
if(m_pathNodes[i].bDisabled)
buf[i/8] |= 1 << i%8;
else
buf[i/8] &= ~(1 << i%8);
for(i = 0; i < m_numPathNodes; i++)
if(m_pathNodes[i].bBetweenLevels)
buf[i/8 + n] |= 1 << i%8;
else
buf[i/8 + n] &= ~(1 << i%8);
}
void
CPathFind::Load(uint8 *buf, uint32 size)
{
int i;
int n = m_numPathNodes/8 + 1;
for(i = 0; i < m_numPathNodes; i++)
if(buf[i/8] & (1 << i%8))
m_pathNodes[i].bDisabled = true;
else
m_pathNodes[i].bDisabled = false;
for(i = 0; i < m_numPathNodes; i++)
if(buf[i/8 + n] & (1 << i%8))
m_pathNodes[i].bBetweenLevels = true;
else
m_pathNodes[i].bBetweenLevels = false;
}
void
CPathFind::DisplayPathData(void)
{
// Not the function from mobile but my own!
int i, j, k;
// Draw 50 units around camera
CVector pos = TheCamera.GetPosition();
const float maxDist = 50.0f;
// Render car path nodes
if(gbShowCarPaths)
for(i = 0; i < m_numCarPathNodes; i++){
if((m_pathNodes[i].GetPosition() - pos).MagnitudeSqr() > SQR(maxDist))
continue;
CVector n1 = m_pathNodes[i].GetPosition();
n1.z += 0.3f;
// Draw node itself
CLines::RenderLineWithClipping(n1.x, n1.y, n1.z,
n1.x, n1.y, n1.z + 1.0f,
0xFFFFFFFF, 0xFFFFFFFF);
for(j = 0; j < m_pathNodes[i].numLinks; j++){
k = ConnectedNode(m_pathNodes[i].firstLink + j);
CVector n2 = m_pathNodes[k].GetPosition();
n2.z += 0.3f;
// Draw links to neighbours
CLines::RenderLineWithClipping(n1.x, n1.y, n1.z,
n2.x, n2.y, n2.z,
0xFFFFFFFF, 0xFFFFFFFF);
}
}
// Render car path nodes
if(gbShowCarPathsLinks)
for(i = 0; i < m_numCarPathLinks; i++){
CVector2D n1_2d = m_carPathLinks[i].GetPosition();
if((n1_2d - pos).MagnitudeSqr() > SQR(maxDist))
continue;
int ni = m_carPathLinks[i].pathNodeIndex;
CVector pn1 = m_pathNodes[ni].GetPosition();
pn1.z += 0.3f;
CVector n1(n1_2d.x, n1_2d.y, pn1.z);
n1.z += 0.3f;
// Draw car node itself
CLines::RenderLineWithClipping(n1.x, n1.y, n1.z,
n1.x, n1.y, n1.z + 1.0f,
0xFFFFFFFF, 0xFFFFFFFF);
CLines::RenderLineWithClipping(n1.x, n1.y, n1.z + 0.5f,
n1.x+m_carPathLinks[i].GetDirX(), n1.y+m_carPathLinks[i].GetDirY(), n1.z + 0.5f,
0xFFFFFFFF, 0xFFFFFFFF);
// Draw connection to car path node
CLines::RenderLineWithClipping(n1.x, n1.y, n1.z,
pn1.x, pn1.y, pn1.z,
0xFF0000FF, 0xFFFFFFFF);
// traffic light type
uint32 col = 0xFF;
if((m_carPathLinks[i].trafficLightType&0x7F) == 1)
col += 0xFF000000;
if((m_carPathLinks[i].trafficLightType&0x7F) == 2)
col += 0x00FF0000;
if(m_carPathLinks[i].trafficLightType & 0x80)
col += 0x0000FF00;
CLines::RenderLineWithClipping(n1.x+0.2f, n1.y, n1.z,
n1.x+0.2f, n1.y, n1.z + 1.0f,
col, col);
for(j = 0; j < m_pathNodes[ni].numLinks; j++){
k = m_carPathConnections[m_pathNodes[ni].firstLink + j];
CVector2D n2_2d = m_carPathLinks[k].GetPosition();
int nk = m_carPathLinks[k].pathNodeIndex;
CVector pn2 = m_pathNodes[nk].GetPosition();
pn2.z += 0.3f;
CVector n2(n2_2d.x, n2_2d.y, pn2.z);
n2.z += 0.3f;
// Draw links to neighbours
CLines::RenderLineWithClipping(n1.x, n1.y, n1.z,
n2.x, n2.y, n2.z,
0xFF00FFFF, 0xFF00FFFF);
}
}
// Render ped path nodes
if(gbShowPedPaths)
for(i = m_numCarPathNodes; i < m_numPathNodes; i++){
if((m_pathNodes[i].GetPosition() - pos).MagnitudeSqr() > SQR(maxDist))
continue;
CVector n1 = m_pathNodes[i].GetPosition();
n1.z += 0.3f;
// Draw node itself
CLines::RenderLineWithClipping(n1.x, n1.y, n1.z,
n1.x, n1.y, n1.z + 1.0f,
0xFFFFFFFF, 0xFFFFFFFF);
for(j = 0; j < m_pathNodes[i].numLinks; j++){
k = ConnectedNode(m_pathNodes[i].firstLink + j);
CVector n2 = m_pathNodes[k].GetPosition();
n2.z += 0.3f;
// Draw links to neighbours
CLines::RenderLineWithClipping(n1.x, n1.y, n1.z,
n2.x, n2.y, n2.z,
0xFFFFFFFF, 0xFFFFFFFF);
// Draw connection flags
CVector mid = (n1+n2)/2.0f;
uint32 col = 0xFF;
if(ConnectionCrossesRoad(m_pathNodes[i].firstLink + j))
col += 0x00FF0000;
if(ConnectionHasTrafficLight(m_pathNodes[i].firstLink + j))
col += 0xFF000000;
CLines::RenderLineWithClipping(mid.x, mid.y, mid.z,
mid.x, mid.y, mid.z + 1.0f,
col, col);
}
}
}
CVector
CPathFind::TakeWidthIntoAccountForWandering(CPathNode* nextNode, uint16 random)
{
CVector pos = nextNode->GetPosition();
float newX = (nextNode->GetPedNodeWidth() * ((random % 16) - 7)) + pos.x;
float newY = (nextNode->GetPedNodeWidth() * (((random / 16) % 16) - 7)) + pos.y;
return CVector(newX, newY, pos.z);
}
void
CPathFind::TakeWidthIntoAccountForCoors(CPathNode* node1, CPathNode* node2, uint16 random, float* x, float* y)
{
*x += (Min(node1->width, node2->width) * WIDTH_TO_PED_NODE_WIDTH * ((random % 16) - 7));
*y += (Min(node1->width, node2->width) * WIDTH_TO_PED_NODE_WIDTH * (((random / 16) % 16) - 7));
}
CPathNode*
CPathFind::GetNode(int16 index)
{
if(index < 0)
return nil;
if(index < ARRAY_SIZE(ThePaths.m_searchNodes))
return &ThePaths.m_searchNodes[index];
return &ThePaths.m_pathNodes[index - ARRAY_SIZE(ThePaths.m_searchNodes)];
}
int16
CPathFind::GetIndex(CPathNode *node)
{
if(node == nil)
return -1;
if(node >= &ThePaths.m_searchNodes[0] && node < &ThePaths.m_searchNodes[ARRAY_SIZE(ThePaths.m_searchNodes)])
return node - ThePaths.m_searchNodes;
else
return (node - ThePaths.m_pathNodes) + ARRAY_SIZE(ThePaths.m_searchNodes);
}