dtPolyRef PathInfo::getPolyByLocation(const float* point, float *distance) { // first we check the current path // if the current path doesn't contain the current poly, // we need to use the expensive navMesh.findNearestPoly dtPolyRef polyRef = getPathPolyByPosition(m_pathPolyRefs, m_polyLength, point, distance); if(polyRef != INVALID_POLYREF) return polyRef; // we don't have it in our old path // try to get it by findNearestPoly() // first try with low search box float extents[VERTEX_SIZE] = {3.0f, 5.0f, 3.0f}; // bounds of poly search area float closestPoint[VERTEX_SIZE] = {0.0f, 0.0f, 0.0f}; dtStatus result = m_navMeshQuery->findNearestPoly(point, extents, &m_filter, &polyRef, closestPoint); if(DT_SUCCESS == result && polyRef != INVALID_POLYREF) { *distance = dtVdist(closestPoint, point); return polyRef; } // still nothing .. // try with bigger search box extents[1] = 200.0f; result = m_navMeshQuery->findNearestPoly(point, extents, &m_filter, &polyRef, closestPoint); if(DT_SUCCESS == result && polyRef != INVALID_POLYREF) { *distance = dtVdist(closestPoint, point); return polyRef; } return INVALID_POLYREF; }
void PathInfo::BuildPolyPath(PathNode startPos, PathNode endPos) { // *** getting start/end poly logic *** float distToStartPoly, distToEndPoly; // first we check the current path // if the current path doesn't contain the current poly, // we need to use the expensive navMesh.findNearestPoly dtPolyRef startPoly = getPathPolyByPosition(m_pathPolyRefs, m_polyLength, startPos, &distToStartPoly); dtPolyRef endPoly = getPathPolyByPosition(m_pathPolyRefs, m_polyLength, endPos, &distToEndPoly); float startPoint[VERTEX_SIZE] = {startPos.y, startPos.z, startPos.x}; float endPoint[VERTEX_SIZE] = {endPos.y, endPos.z, endPos.x}; // we don't have it in our old path // try to get it by findNearestPoly() // use huge vertical range here if (startPoly == INVALID_POLYREF || endPoly == INVALID_POLYREF) { float extents[VERTEX_SIZE] = {3.f, 200.f, 3.f}; // bounds of poly search area dtQueryFilter filter = createFilter(); float closestPoint[VERTEX_SIZE]; if (startPoly == INVALID_POLYREF) { if (DT_SUCCESS == m_navMeshQuery->findNearestPoly(startPoint, extents, &filter, &startPoly, closestPoint)) distToStartPoly = dtVdist(closestPoint, startPoint); } if (endPoly == INVALID_POLYREF) { if (DT_SUCCESS == m_navMeshQuery->findNearestPoly(endPoint, extents, &filter, &endPoly, closestPoint)) distToEndPoly = dtVdist(closestPoint, endPoint); } } // we have a hole in our mesh // make shortcut path and mark it as NOPATH ( with flying exception ) // its up to caller how he will use this info if (startPoly == INVALID_POLYREF || endPoly == INVALID_POLYREF) { PATH_DEBUG("++ BuildPolyPath :: (startPoly == 0 || endPoly == 0)\n"); BuildShortcut(); m_type = (m_sourceUnit->GetTypeId() == TYPEID_UNIT && ((Creature*)m_sourceUnit)->IsLevitating()) ? PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH) : PATHFIND_NOPATH; return; } // we may need a better number here bool farFromPoly = (distToStartPoly > 7.0f || distToEndPoly > 7.0f); if (farFromPoly) { PATH_DEBUG("++ BuildPolyPath :: farFromPoly distToStartPoly=%.3f distToEndPoly=%.3f\n", distToStartPoly, distToEndPoly); bool buildShotrcut = false; if (m_sourceUnit->GetTypeId() == TYPEID_UNIT) { Creature* owner = (Creature*)m_sourceUnit; if (m_sourceUnit->GetBaseMap()->IsUnderWater(endPos.x, endPos.y, endPos.z)) { PATH_DEBUG("++ BuildPolyPath :: underWater case\n"); if (owner->canSwim()) buildShotrcut = true; } else { PATH_DEBUG("++ BuildPolyPath :: flying case\n"); if (owner->IsLevitating()) buildShotrcut = true; } } if (buildShotrcut) { BuildShortcut(); m_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH); return; } else { float closestPoint[VERTEX_SIZE]; // we may want to use closestPointOnPolyBoundary instead if (DT_SUCCESS == m_navMeshQuery->closestPointOnPoly(endPoly, endPoint, closestPoint)) { dtVcopy(endPoint, closestPoint); setActualEndPosition(PathNode(endPoint[2],endPoint[0],endPoint[1])); } m_type = PATHFIND_INCOMPLETE; } } // *** poly path generating logic *** // start and end are on same polygon // just need to move in straight line if (startPoly == endPoly) { PATH_DEBUG("++ BuildPolyPath :: (startPoly == endPoly)\n"); BuildShortcut(); m_pathPolyRefs = new dtPolyRef[1]; m_pathPolyRefs[0] = startPoly; m_polyLength = 1; m_type = farFromPoly ? PATHFIND_INCOMPLETE : PATHFIND_NORMAL; PATH_DEBUG("++ BuildPolyPath :: path type %d\n", m_type); return; } // look for startPoly/endPoly in current path // TODO: we can merge it with getPathPolyByPosition() loop bool startPolyFound = false; bool endPolyFound = false; uint32 pathStartIndex, pathEndIndex; if (m_polyLength) { for (pathStartIndex = 0; pathStartIndex < m_polyLength; ++pathStartIndex) { if (m_pathPolyRefs[pathStartIndex] == startPoly) { startPolyFound = true; break; } for (pathEndIndex = m_polyLength-1; pathEndIndex > pathStartIndex; --pathEndIndex) { if (m_pathPolyRefs[pathEndIndex] == endPoly) { endPolyFound = true; break; } } } } if (startPolyFound && endPolyFound) { PATH_DEBUG("++ BuildPolyPath :: (startPolyFound && endPolyFound)\n"); // we moved along the path and the target did not move out of our old poly-path // our path is a simple subpath case, we have all the data we need // just "cut" it out m_polyLength = pathEndIndex - pathStartIndex + 1; dtPolyRef* newPolyRefs = new dtPolyRef[m_polyLength]; memcpy(newPolyRefs, m_pathPolyRefs+pathStartIndex, m_polyLength*sizeof(dtPolyRef)); delete [] m_pathPolyRefs; m_pathPolyRefs = newPolyRefs; } else if (startPolyFound && !endPolyFound) { PATH_DEBUG("++ BuildPolyPath :: (startPolyFound && !endPolyFound)\n"); // we are moving on the old path but target moved out // so we have atleast part of poly-path ready m_polyLength -= pathStartIndex; // try to adjust the suffix of the path instead of recalculating entire length // at given interval the target cannot get too far from its last location // thus we have less poly to cover // sub-path of optimal path is optimal // take ~80% of the original length // TODO : play with the values here uint32 prefixPolyLength = uint32(m_polyLength*0.8f + 0.5f); dtPolyRef prefixPathPolys[MAX_PATH_LENGTH]; memcpy(prefixPathPolys, m_pathPolyRefs+pathStartIndex, prefixPolyLength*sizeof(dtPolyRef)); dtPolyRef suffixStartPoly = prefixPathPolys[prefixPolyLength-1]; // we need any point on our suffix start poly to generate poly-path, so we need last poly in prefix data float suffixEndPoint[VERTEX_SIZE]; if (DT_SUCCESS != m_navMeshQuery->closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint)) { // suffixStartPoly is invalid somehow, or the navmesh is broken => error state sLog->outError("%u's Path Build failed: invalid polyRef in path", m_sourceUnit->GetGUID()); BuildShortcut(); m_type = PATHFIND_NOPATH; return; } // generate suffix dtQueryFilter filter = createFilter(); dtPolyRef suffixPathPolys[MAX_PATH_LENGTH]; uint32 suffixPolyLength = 0; dtStatus dtResult = m_navMeshQuery->findPath( suffixStartPoly, // start polygon endPoly, // end polygon suffixEndPoint, // start position endPoint, // end position &filter, // polygon search filter suffixPathPolys, // [out] path (int*)&suffixPolyLength, MAX_PATH_LENGTH-prefixPolyLength); // max number of polygons in output path if (!suffixPolyLength || dtResult != DT_SUCCESS) { // this is probably an error state, but we'll leave it // and hopefully recover on the next Update // we still need to copy our preffix sLog->outError("%u's Path Build failed: 0 length path", m_sourceUnit->GetGUID()); } PATH_DEBUG("++ m_polyLength=%u prefixPolyLength=%u suffixPolyLength=%u \n",m_polyLength, prefixPolyLength, suffixPolyLength); // new path = prefix + suffix - overlap m_polyLength = prefixPolyLength + suffixPolyLength - 1; delete [] m_pathPolyRefs; m_pathPolyRefs = new dtPolyRef[m_polyLength]; // copy the part of the old path we keep - prefix memcpy(m_pathPolyRefs, prefixPathPolys, prefixPolyLength*sizeof(dtPolyRef)); // copy the newly created suffix - skip first poly, we have it at prefix end if (suffixPathPolys) memcpy(m_pathPolyRefs+prefixPolyLength, suffixPathPolys+1, (suffixPolyLength-1)*sizeof(dtPolyRef)); } else { PATH_DEBUG("++ BuildPolyPath :: (!startPolyFound && !endPolyFound)\n"); // either we have no path at all -> first run // or something went really wrong -> we aren't moving along the path to the target // just generate new path // free and invalidate old path data clear(); dtQueryFilter filter = createFilter(); // use special filter so we use proper terrain types dtPolyRef pathPolys[MAX_PATH_LENGTH]; m_polyLength = 0; dtStatus dtResult = m_navMeshQuery->findPath( startPoly, // start polygon endPoly, // end polygon startPoint, // start position endPoint, // end position &filter, // polygon search filter pathPolys, // [out] path (int*)&m_polyLength, MAX_PATH_LENGTH); // max number of polygons in output path if (!m_polyLength || dtResult != DT_SUCCESS) { // only happens if we passed bad data to findPath(), or navmesh is messed up sLog->outError("%u's Path Build failed: 0 length path", m_sourceUnit->GetGUID()); BuildShortcut(); m_type = PATHFIND_NOPATH; return; } m_pathPolyRefs = new dtPolyRef[m_polyLength]; memcpy(m_pathPolyRefs, pathPolys, m_polyLength*sizeof(dtPolyRef)); } // by now we know what type of path we can get if (m_pathPolyRefs[m_polyLength - 1] == endPoly && !(m_type & PATHFIND_INCOMPLETE)) m_type = PATHFIND_NORMAL; else m_type = PATHFIND_INCOMPLETE; // generate the point-path out of our up-to-date poly-path BuildPointPath(startPoint, endPoint); }
void PathInfo::Update(const float destX, const float destY, const float destZ) { float x, y, z; // update start and end m_sourceObject->GetPosition(x, y, z); setStartPosition(x, y, z); setEndPosition(destX, destY, destZ); // make sure navMesh works if(!m_navMesh) { m_sourceObject->GetPosition(x, y, z); m_navMesh = m_sourceObject->GetMap()->GetNavMesh(); if(!m_navMesh) { // can't pathfind if navmesh doesn't exist shortcut(); return; } } if(!m_pathPolyRefs) { // path was not built before, most likely because navmesh wasn't working // start from scratch, then return Build(); return; } // should be safe to update path now bool startOffPath = false; bool endOffPath = false; // find start and end poly // navMesh.findNearestPoly is expensive, so first we check just the current path getStartPosition(x, y, z); dtPolyRef startPoly = getPathPolyByPosition(x, y, z); getEndPosition(x, y, z); dtPolyRef endPoly = getPathPolyByPosition(x, y, z); if(startPoly != 0 && endPoly != 0) trim(startPoly, endPoly); else { // start or end is off the path, need to find the polygon float extents[3] = {2.f, 4.f, 2.f}; // bounds of poly search area dtQueryFilter filter = dtQueryFilter(); // filter for poly search if(!startPoly) { getStartPosition(x, y, z); float startPos[3] = {y, z, x}; startOffPath = true; startPoly = m_navMesh->findNearestPoly(startPos, extents, &filter, 0); } if(!endPoly) { getEndPosition(x, y, z); float endPos[3] = {y, z, x}; endOffPath = true; endPoly = m_navMesh->findNearestPoly(endPos, extents, &filter, 0); } if(startPoly == 0 || endPoly == 0) { // source or dest not near navmesh polygons: // flying, falling, swimming, or navmesh has a hole // ignore obstacles/terrain is better than giving up // PATHFIND TODO: prevent walking/swimming mobs from flying into the air shortcut(); return; } } if(startPoly == endPoly) { // start and end are on same polygon // just need to move in straight line // PATHFIND TODO: prevent walking/swimming mobs from flying into the air clear(); m_pathPolyRefs = new dtPolyRef[1]; m_pathPolyRefs[0] = startPoly; m_length = 1; getEndPosition(x, y, z); setNextPosition(x, y, z); m_type = PathType(m_type | PATHFIND_NORMAL); return; } if(startOffPath) { bool adjacent = false; int i; for(i = 0; i < DT_VERTS_PER_POLYGON; ++i) if(startPoly == m_navMesh->getPolyByRef(m_pathPolyRefs[0])->neis[i]) { adjacent = true; break; } if(adjacent) { // startPoly is adjacent to the path, we can add it to the start of the path // 50th poly will fall off of path, shouldn't be an issue because most paths aren't that long m_length = m_length < MAX_PATH_LENGTH ? m_length + 1 : m_length; dtPolyRef* temp = new dtPolyRef[m_length]; temp[0] = startPoly; for(i = 1; i < m_length; ++i) temp[i] = m_pathPolyRefs[i - 1]; delete [] m_pathPolyRefs; m_pathPolyRefs = temp; } else { // waste of time to optimize, just find brand new path Build(startPoly, endPoly); return; } } if(endOffPath) { bool adjacent = false; int i; for(i = 0; i < DT_VERTS_PER_POLYGON; ++i) if(startPoly == m_navMesh->getPolyByRef(m_pathPolyRefs[0])->neis[i]) { adjacent = true; break; } if(adjacent) { if(m_length < MAX_PATH_LENGTH) { // endPoly is adjacent to the path, and we have enough room to add it to the end dtPolyRef* temp = new dtPolyRef[m_length + 1]; for(i = 0; i < m_length; ++i) temp[i] = m_pathPolyRefs[i]; temp[i] = endPoly; delete [] m_pathPolyRefs; m_pathPolyRefs = temp; } //else // ; // endPoly is adjacent to the path, we just don't have room to store it } else { // waste of time to optimize, just find brand new path Build(startPoly, endPoly); return; } } updateNextPosition(); }