Method FindPath

Source/Engine/Navigation/NavMeshRuntime.cpp:94–156

}

bool NavMeshRuntime::FindPath(const Vector3& startPosition, const Vector3& endPosition, Array<Vector3, HeapAllocation>& resultPath, NavMeshPathFlags& resultFlags) const
{
    resultPath.Clear();
    resultFlags = NavMeshPathFlags::None;
    ScopeLock lock(Locker);
    const auto query = GetNavMeshQuery();
    if (!query || !_navMesh)
        return false;

    dtQueryFilter filter;
    InitFilter(filter);
    Float3 extent = Properties.DefaultQueryExtent;

    Float3 startPositionNavMesh, endPositionNavMesh;
    Float3::Transform(startPosition, Properties.Rotation, startPositionNavMesh);
    Float3::Transform(endPosition, Properties.Rotation, endPositionNavMesh);

    dtPolyRef startPoly = 0;
    if (!dtStatusSucceed(query->findNearestPoly(&startPositionNavMesh.X, &extent.X, &filter, &startPoly, nullptr)))
        return false;
    dtPolyRef endPoly = 0;
    if (!dtStatusSucceed(query->findNearestPoly(&endPositionNavMesh.X, &extent.X, &filter, &endPoly, nullptr)))
        return false;

    dtPolyRef path[NAV_MESH_PATH_MAX_SIZE];
    int32 pathSize;
    const auto findPathStatus = query->findPath(startPoly, endPoly, &startPositionNavMesh.X, &endPositionNavMesh.X, &filter, path, &pathSize, NAV_MESH_PATH_MAX_SIZE);
    if (dtStatusFailed(findPathStatus))
    {
        return false;
    }

    Quaternion invRotation;
    Quaternion::Invert(Properties.Rotation, invRotation);

    if (pathSize == 1 && dtStatusDetail(findPathStatus, DT_PARTIAL_RESULT))
    {
        resultFlags |= NavMeshPathFlags::PartialPath;
        // TODO: skip adding 2nd end point if it's not reachable (use navmesh raycast check? or physics check? or local Z distance check?)
        resultPath.Resize(2);
        resultPath[0] = startPosition;
        query->closestPointOnPolyBoundary(startPoly, &endPositionNavMesh.X, &endPositionNavMesh.X);
        resultPath[1] = endPositionNavMesh;
        Vector3::Transform(resultPath[1], invRotation, resultPath[1]);
    }
    else
    {
        int pathPointsCount = 0;
        Float3 pathPoints[NAV_MESH_PATH_MAX_SIZE];
        const auto findStraightPathStatus = query->findStraightPath(&startPositionNavMesh.X, &endPositionNavMesh.X, path, pathSize, (float*)&pathPoints, nullptr, nullptr, &pathPointsCount, NAV_MESH_PATH_MAX_SIZE, DT_STRAIGHTPATH_AREA_CROSSINGS);
        if (dtStatusFailed(findStraightPathStatus))
        {
            return false;
        }
        resultPath.Resize(pathPointsCount);
        for (int32 i = 0; i < pathPointsCount; i++)
        {
            Vector3::Transform(pathPoints[i], invRotation, resultPath[i]);