diff options
Diffstat (limited to '')
| -rw-r--r-- | src/control/PathFind.cpp | 370 |
1 files changed, 350 insertions, 20 deletions
diff --git a/src/control/PathFind.cpp b/src/control/PathFind.cpp index 608a209a..9f45c454 100644 --- a/src/control/PathFind.cpp +++ b/src/control/PathFind.cpp @@ -5,13 +5,17 @@ #include "Camera.h" #include "Vehicle.h" #include "World.h" +#include "Lines.h" // for debug #include "PathFind.h" -CPathFind &ThePaths = *(CPathFind*)0x8F6754; +bool gbShowPedPaths; +bool gbShowCarPaths; +bool gbShowCarPathsLinks; -WRAPPER bool CPedPath::CalcPedRoute(uint8, CVector, CVector, CVector*, int16*, int16) { EAXJMP(0x42E680); } +CPathFind &ThePaths = *(CPathFind*)0x8F6754; #define MAX_DIST INT16_MAX-1 +#define MIN_PED_ROUTE_DISTANCE 23.8f // object flags: // 1 UseInRoadBlock @@ -23,6 +27,199 @@ CPathInfoForObject *&InfoForTilePeds = *(CPathInfoForObject**)0x8F1AE4; CTempDetachedNode *&DetachedNodesCars = *(CTempDetachedNode**)0x8E2824; CTempDetachedNode *&DetachedNodesPeds = *(CTempDetachedNode**)0x8E28A0; +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 CColBox& 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; + } + } + } + } + } +} + void CPathFind::Init(void) { @@ -466,20 +663,20 @@ CPathFind::PreparePathDataForType(uint8 type, CTempNode *tempnodes, CPathInfoFor // 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].posX == tempnodes[j].pos.x && - m_carPathLinks[k].posY == tempnodes[j].pos.y){ + if(m_carPathLinks[k].dir.x == tempnodes[j].dirX && + m_carPathLinks[k].dir.y == tempnodes[j].dirY && + m_carPathLinks[k].pos.x == tempnodes[j].pos.x && + m_carPathLinks[k].pos.y == tempnodes[j].pos.y){ 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].posX = tempnodes[j].pos.x; - m_carPathLinks[m_numCarPathLinks].posY = tempnodes[j].pos.y; + m_carPathLinks[m_numCarPathLinks].dir.x = tempnodes[j].dirX; + m_carPathLinks[m_numCarPathLinks].dir.y = tempnodes[j].dirY; + m_carPathLinks[m_numCarPathLinks].pos.x = tempnodes[j].pos.x; + m_carPathLinks[m_numCarPathLinks].pos.y = tempnodes[j].pos.y; m_carPathLinks[m_numCarPathLinks].pathNodeIndex = i; m_carPathLinks[m_numCarPathLinks].numLeftLanes = tempnodes[j].numLeftLanes; m_carPathLinks[m_numCarPathLinks].numRightLanes = tempnodes[j].numRightLanes; @@ -529,20 +726,20 @@ CPathFind::PreparePathDataForType(uint8 type, CTempNode *tempnodes, CPathInfoFor // IMPROVE: use a goto here // Find existing car path link for(k = 0; k < m_numCarPathLinks; k++){ - if(m_carPathLinks[k].dirX == dx && - m_carPathLinks[k].dirY == dy && - m_carPathLinks[k].posX == posx && - m_carPathLinks[k].posY == posy){ + if(m_carPathLinks[k].dir.x == dx && + m_carPathLinks[k].dir.y == dy && + m_carPathLinks[k].pos.x == posx && + m_carPathLinks[k].pos.y == posy){ 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; - m_carPathLinks[m_numCarPathLinks].dirY = dy; - m_carPathLinks[m_numCarPathLinks].posX = posx; - m_carPathLinks[m_numCarPathLinks].posY = posy; + m_carPathLinks[m_numCarPathLinks].dir.x = dx; + m_carPathLinks[m_numCarPathLinks].dir.y = dy; + m_carPathLinks[m_numCarPathLinks].pos.x = posx; + m_carPathLinks[m_numCarPathLinks].pos.y = posy; m_carPathLinks[m_numCarPathLinks].pathNodeIndex = i; m_carPathLinks[m_numCarPathLinks].numLeftLanes = -1; m_carPathLinks[m_numCarPathLinks].numRightLanes = -1; @@ -760,8 +957,8 @@ CPathFind::SetLinksBridgeLights(float x1, float x2, float y1, float y2, bool ena { int i; for(i = 0; i < m_numCarPathLinks; i++) - if(x1 < m_carPathLinks[i].posX && m_carPathLinks[i].posX < x2 && - y1 < m_carPathLinks[i].posY && m_carPathLinks[i].posY < y2) + if(x1 < m_carPathLinks[i].pos.x && m_carPathLinks[i].pos.x < x2 && + y1 < m_carPathLinks[i].pos.y && m_carPathLinks[i].pos.y < y2) m_carPathLinks[i].bBridgeLights = enable; } @@ -1444,7 +1641,140 @@ CPathFind::Load(uint8 *buf, uint32 size) m_pathNodes[i].bBetweenLevels = false; } +void +CPathFind::DisplayPathData(void) +{ + // Not the function from mobm_carPathLinksile 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].pos - pos).MagnitudeSqr() > SQR(maxDist)) + continue; + + CVector n1 = m_pathNodes[i].pos; + 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 = m_connections[m_pathNodes[i].firstLink + j]; + CVector n2 = m_pathNodes[k].pos; + 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].pos; + if((n1_2d - pos).MagnitudeSqr() > SQR(maxDist)) + continue; + + int ni = m_carPathLinks[i].pathNodeIndex; + CVector pn1 = m_pathNodes[ni].pos; + 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].dir.x, n1.y+m_carPathLinks[i].dir.y, 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].pos; + int nk = m_carPathLinks[k].pathNodeIndex; + CVector pn2 = m_pathNodes[nk].pos; + 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].pos - pos).MagnitudeSqr() > SQR(maxDist)) + continue; + + CVector n1 = m_pathNodes[i].pos; + 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 = m_connections[m_pathNodes[i].firstLink + j]; + CVector n2 = m_pathNodes[k].pos; + 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(m_connectionFlags[m_pathNodes[i].firstLink + j].bCrossesRoad) + col += 0x00FF0000; + if(m_connectionFlags[m_pathNodes[i].firstLink + j].bTrafficLight) + col += 0xFF000000; + CLines::RenderLineWithClipping(mid.x, mid.y, mid.z, + mid.x, mid.y, mid.z + 1.0f, + col, col); + } + } +} + STARTPATCHES + InjectHook(0x42E680, &CPedPath::CalcPedRoute, PATCH_JUMP); + InjectHook(0x42F100, &CPedPath::AddNodeToPathList, PATCH_JUMP); + InjectHook(0x42F140, &CPedPath::RemoveNodeFromList, PATCH_JUMP); + InjectHook(0x42F160, &CPedPath::AddNodeToList, PATCH_JUMP); + InjectHook(0x42F1A0, &CPedPath::AddBlockade, PATCH_JUMP); + InjectHook(0x42F420, &CPedPath::AddBlockadeSectorList, PATCH_JUMP); + InjectHook(0x4294A0, &CPathFind::Init, PATCH_JUMP); InjectHook(0x42D580, &CPathFind::AllocatePathFindInfoMem, PATCH_JUMP); InjectHook(0x429540, &CPathFind::RegisterMapObject, PATCH_JUMP); |