Exemplo n.º 1
0
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;
}
Exemplo n.º 2
0
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);
}
Exemplo n.º 3
0
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();
}