bool PathGenerator::CalculatePath(float destX, float destY, float destZ, bool forceDest, bool straightLine)
{
    float x, y, z;
    _sourceUnit->GetPosition(x, y, z);

    if (!Trinity::IsValidMapCoord(destX, destY, destZ) || !Trinity::IsValidMapCoord(x, y, z))
        return false;

    G3D::Vector3 dest(destX, destY, destZ);
    SetEndPosition(dest);

    G3D::Vector3 start(x, y, z);
    SetStartPosition(start);

    _forceDestination = forceDest;
    _straightLine = straightLine;

    TC_LOG_DEBUG("maps", "++ PathGenerator::CalculatePath() for %u \n", _sourceUnit->GetGUIDLow());

    // make sure navMesh works - we can run on map w/o mmap
    // check if the start and end point have a .mmtile loaded (can we pass via not loaded tile on the way?)
    if (!_navMesh || !_navMeshQuery || _sourceUnit->HasUnitState(UNIT_STATE_IGNORE_PATHFINDING) ||
        !HaveTile(start) || !HaveTile(dest))
    {
        BuildShortcut();
        _type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
        return true;
    }

    UpdateFilter();

    BuildPolyPath(start, dest);
    return true;
}

bool PathFinder::calculate(float destX, float destY, float destZ, bool forceDest)
{
    Vector3 oldDest = getEndPosition();
    Vector3 dest(destX, destY, destZ);
    setEndPosition(dest);

    float x, y, z;
    m_sourceUnit->GetPosition(x, y, z);
    Vector3 start(x, y, z);
    setStartPosition(start);

    m_forceDestination = forceDest;

    DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ PathFinder::calculate() for %u \n", m_sourceUnit->GetGUIDLow());

    // make sure navMesh works - we can run on map w/o mmap
    // check if the start and end point have a .mmtile loaded (can we pass via not loaded tile on the way?)
    if (!m_navMesh || !m_navMeshQuery || m_sourceUnit->hasUnitState(UNIT_STAT_IGNORE_PATHFINDING) ||
        !HaveTile(start) || !HaveTile(dest) || (m_sourceUnit->GetTypeId() == TYPEID_UNIT && ((Creature*)m_sourceUnit)->IsLevitating()))
    {
        BuildShortcut();
        m_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
        return true;
    }

    updateFilter();

    {
        ReadGuard Guard(MMAP::MMapFactory::createOrGetMMapManager()->GetLock(m_sourceUnit->GetMapId()));
        BuildPolyPath(start, dest);
    }
    return true;
}

bool PathFinder::calculate(float destX, float destY, float destZ, bool forceDest)
{
    Vector3 oldDest = getEndPosition();
    Vector3 dest(destX, destY, destZ);
    setEndPosition(dest);

    float x, y, z;
    m_sourceUnit->GetPosition(x, y, z);
    Vector3 start(x, y, z);
    setStartPosition(start);

    m_forceDestination = forceDest;

    DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ PathFinder::calculate() for %u \n", m_sourceUnit->GetGUIDLow());

    // make sure navMesh works - we can run on map w/o mmap
    // check if the start and end point have a .mmtile loaded (can we pass via not loaded tile on the way?)
    if (!m_navMesh || !m_navMeshQuery || m_sourceUnit->hasUnitState(UNIT_STAT_IGNORE_PATHFINDING) ||
        !HaveTile(start) || !HaveTile(dest) || m_sourceUnit->m_movementInfo.HasMovementFlag(MOVEFLAG_TAXI)) //for transport
    {
        BuildShortcut();
        m_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
        return true;
    }

    updateFilter();

    BuildPolyPath(start, dest);
    return true;
}

////////////////// PathInfo //////////////////
PathInfo::PathInfo(const Unit* owner, const float destX, const float destY, const float destZ, bool useStraightPath) :
    m_pathPolyRefs(NULL), m_polyLength(0), m_type(PATHFIND_BLANK), m_useStraightPath(useStraightPath),
    m_sourceUnit(owner), m_navMesh(NULL), m_navMeshQuery(NULL)
{
    PathNode endPoint(destX, destY, destZ);
    setEndPosition(endPoint);

    float x,y,z;
    m_sourceUnit->GetPosition(x, y, z);
    PathNode startPoint(x, y, m_sourceUnit->GetMap()->GetHeight(x,y,z,100.0f));
    setStartPosition(startPoint);

    PATH_DEBUG("++ PathInfo::PathInfo for %u \n", m_sourceUnit->GetGUID());

    uint32 mapId = m_sourceUnit->GetMapId();
    MMAP::MMapManager* mmap = MMAP::MMapFactory::createOrGetMMapManager();
    m_navMesh = mmap->GetNavMesh(mapId);
    m_navMeshQuery = mmap->GetNavMeshQuery(mapId);

    if (m_navMesh && m_navMeshQuery)
    {
        BuildPolyPath(startPoint, endPoint);
    }
    else
    {
        BuildShortcut();
        m_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
    }
}

bool PathInfo::Update(const float destX, const float destY, const float destZ, bool useStraightPath)
{
    PathNode newDest(destX, destY, m_sourceUnit->GetMap()->GetHeight(destX,destY,destZ,100.0f));
    PathNode oldDest = getEndPosition();
    setEndPosition(newDest);

    float x, y, z;
    m_sourceUnit->GetPosition(x, y, z);
    PathNode newStart(x, y, m_sourceUnit->GetMap()->GetHeight(x,y,z,100.0f));
    PathNode oldStart = getStartPosition();
    setStartPosition(newStart);

    m_useStraightPath = useStraightPath;

    PATH_DEBUG("++ PathInfo::Update() for %u \n", m_sourceUnit->GetGUID());

    // make sure navMesh works - we can run on map w/o mmap
    if (!m_navMesh || !m_navMeshQuery)
    {
        BuildShortcut();
        m_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
        return true;
    }

    float dist = m_sourceUnit->GetObjectBoundingRadius();
    bool oldDestInRange = inRange(oldDest, newDest, dist, dist);

    // this can happen only if caller did a bad job calculating the need for path update
    if (oldDestInRange && inRange(newStart, oldStart, dist, dist))
    {
        setEndPosition(oldDest);
        setStartPosition(oldStart);
        return false;
    }

    // check if destination moved - if not we can optimize something here
    // we are following old, precalculated path?
    if (oldDestInRange && m_pathPoints.size() > 2)
    {
        // our target is not moving - we just coming closer
        // we are moving on precalculated path - enjoy the ride
        PATH_DEBUG("++ PathInfo::Update:: precalculated path\n");

        m_pathPoints.crop(1, 0);
        setNextPosition(m_pathPoints[1]);

        return false;
    }
    else
    {
        // target moved, so we need to update the poly path
        BuildPolyPath(newStart, newDest);
        return true;
    }
}

bool PathFinderMovementGenerator::calculate(float destX, float destY, float destZ, bool forceDest)
{
    if (!SkyFire::IsValidMapCoord(destX, destY, destZ) ||
        !SkyFire::IsValidMapCoord(m_sourceUnit->GetPositionX(), m_sourceUnit->GetPositionY(), m_sourceUnit->GetPositionZ()))
        return false;

    Vector3 oldDest = getEndPosition();
    Vector3 dest(destX, destY, destZ);
    setEndPosition(dest);

    float x, y, z;
    m_sourceUnit->GetPosition(x, y, z);
    Vector3 start(x, y, z);
    setStartPosition(start);

    m_forceDestination = forceDest;

    sLog->outDebug(LOG_FILTER_MAPS, "++ PathFinderMovementGenerator::calculate() for %u \n", m_sourceUnit->GetGUIDLow());

    // make sure navMesh works - we can run on map w/o mmap
    // check if the start and end point have a .mmtile loaded (can we pass via not loaded tile on the way?)
    if (!m_navMesh || !m_navMeshQuery || m_sourceUnit->HasUnitState(UNIT_STATE_IGNORE_PATHFINDING) ||
        !HaveTile(start) || !HaveTile(dest))
    {
        BuildShortcut();
        m_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
        return true;
    }

    updateFilter();

    // check if destination moved - if not we can optimize something here
    // we are following old, precalculated path?
    float dist = m_sourceUnit->GetObjectSize();
    if (inRange(oldDest, dest, dist, dist) && m_pathPoints.size() > 2)
    {
        // our target is not moving - we just coming closer
        // we are moving on precalculated path - enjoy the ride
        sLog->outStaticDebug("++ PathFinderMovementGenerator::calculate:: precalculated path\n");

        m_pathPoints.erase(m_pathPoints.begin());
        return false;
    }
    else
    {
        // target moved, so we need to update the poly path
        m_navMeshLock->acquire_read();
        BuildPolyPath(start, dest);
        m_navMeshLock->release();
        return true;
    }
}

bool PathGenerator::CalculatePath(float destX, float destY, float destZ, bool forceDest)
{
    float x, y, z;
    _sourceUnit->GetPosition(x, y, z);

    if (!Trinity::IsValidMapCoord(destX, destY, destZ) || !Trinity::IsValidMapCoord(x, y, z))
        return false;

    Vector3 oldDest = GetEndPosition();
    Vector3 dest(destX, destY, destZ);
    SetEndPosition(dest);

    Vector3 start(x, y, z);
    SetStartPosition(start);

    _forceDestination = forceDest;

    sLog->outDebug(LOG_FILTER_MAPS, "++ PathGenerator::CalculatePath() for %u \n", _sourceUnit->GetGUIDLow());

    // make sure navMesh works - we can run on map w/o mmap
    // check if the start and end point have a .mmtile loaded (can we pass via not loaded tile on the way?)
    if (!_navMesh || !_navMeshQuery || _sourceUnit->HasUnitState(UNIT_STATE_IGNORE_PATHFINDING) ||
        !HaveTile(start) || !HaveTile(dest))
    {
        BuildShortcut();
        _type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
        return true;
    }

    UpdateFilter();

    // check if destination moved - if not we can optimize something here
    // we are following old, precalculated path?
    float dist = _sourceUnit->GetObjectSize();
    if (oldDest != Vector3::zero() && InRange(oldDest, dest, dist, dist) && _pathPoints.size() > 2)
    {
        // our target is not moving - we just coming closer
        // we are moving on precalculated path - enjoy the ride
        sLog->outDebug(LOG_FILTER_MAPS, "++ PathGenerator::CalculatePath:: precalculated path\n");

        _pathPoints.erase(_pathPoints.begin());
        return false;
    }
    else
    {
        // target moved, so we need to update the poly path
        BuildPolyPath(start, dest);
        return true;
    }
}

bool PathInfo::Update(const float destX, const float destY, const float destZ,
                      bool useStraightPath, bool forceDest)
{
    PathNode newDest(destX, destY, destZ);
    PathNode oldDest = getEndPosition();
    setEndPosition(newDest);

    float x, y, z;
    m_sourceUnit->GetPosition(x, y, z);
    PathNode newStart(x, y, z);
    PathNode oldStart = getStartPosition();
    setStartPosition(newStart);

    m_useStraightPath = useStraightPath;
    m_forceDestination = forceDest;

    DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ PathInfo::Update() for %u \n", m_sourceUnit->GetGUID());

    // make sure navMesh works - we can run on map w/o mmap
    if (!m_navMesh || !m_navMeshQuery || !HaveTiles(newDest) ||
            m_sourceUnit->hasUnitState(UNIT_STAT_IGNORE_PATHFINDING))
    {
        BuildShortcut();
        m_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
        return true;
    }

    updateFilter();

    // check if destination moved - if not we can optimize something here
    // we are following old, precalculated path?
    float dist = m_sourceUnit->GetObjectBoundingRadius();
    if (inRange(oldDest, newDest, dist, dist) && m_pathPoints.size() > 2)
    {
        // our target is not moving - we just coming closer
        // we are moving on precalculated path - enjoy the ride
        DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ PathInfo::Update:: precalculated path\n");

        m_pathPoints.crop(1, 0);
        setNextPosition(m_pathPoints[1]);

        return false;
    }
    else
    {
        // target moved, so we need to update the poly path
        BuildPolyPath(newStart, newDest);
        return true;
    }
}

bool PathFinder::calculate(float destX, float destY, float destZ, bool forceDest)
{
    Vector3 oldDest = getEndPosition();
    Vector3 dest(destX, destY, destZ);
    setEndPosition(dest);

    float x, y, z;
    m_sourceUnit->GetPosition(x, y, z);
    Vector3 start(x, y, z);
    setStartPosition(start);

    m_forceDestination = forceDest;

    DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ PathFinder::calculate() for %u \n", m_sourceUnit->GetGUIDLow());

    // make sure navMesh works - we can run on map w/o mmap
    // check if the start and end point have a .mmtile loaded (can we pass via not loaded tile on the way?)
    if (!m_navMesh || !m_navMeshQuery || m_sourceUnit->hasUnitState(UNIT_STAT_IGNORE_PATHFINDING) ||
        !HaveTile(start) || !HaveTile(dest))
    {
        BuildShortcut();
        m_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
        return true;
    }

    updateFilter();

    // check if destination moved - if not we can optimize something here
    // we are following old, precalculated path?
    float dist = m_sourceUnit->GetObjectBoundingRadius();
    if (inRange(oldDest, dest, dist, dist) && m_pathPoints.size() > 2)
    {
        // our target is not moving - we just coming closer
        // we are moving on precalculated path - enjoy the ride
        DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ PathFinder::calculate:: precalculated path\n");

        m_pathPoints.erase(m_pathPoints.begin());
        return false;
    }
    else
    {
        // target moved, so we need to update the poly path
        ReadGuard Guard(MMAP::MMapFactory::createOrGetMMapManager()->GetLock(m_sourceUnit->GetMapId()));
        BuildPolyPath(start, dest);
        return true;
    }
}

/**
 * @brief Class destructor.
 *
 * condor2nav::COStream class destructor. Writes local buffer to
 * a file.
 */
condor2nav::COStream::~COStream()
{
  if(_buffer.str().size()) {
    for(auto &path : _pathList) {
      switch(PathType(path)) {
      case TPathType::LOCAL:
        {
          bfs::ofstream stream{path, std::ios_base::out | std::ios_base::binary};
          if(!stream)
            throw EOperationFailed{"ERROR: Couldn't open file '" + path.string() + "' for writing!!!"};
          stream << _buffer.str();
        }
        break;

      case TPathType::ACTIVE_SYNC:
        CActiveSync::Instance().Write(path, _buffer.str());
        break;
      }
    }
  }
}

bool PathGenerator::CalculatePath(float destX, float destY, float destZ, bool /*forceDest*/)
{
    float x, y, z;
    _sourceUnit->GetPosition(x, y, z);

    if (!Trinity::IsValidMapCoord(destX, destY, destZ) || !Trinity::IsValidMapCoord(x, y, z))
        return false;

    G3D::Vector3 dest(destX, destY, destZ);
    SetEndPosition(dest);
    SetActualEndPosition(GetEndPosition());

    G3D::Vector3 start(x, y, z);
    SetStartPosition(start);

    TC_LOG_DEBUG("maps", "++ PathGenerator::CalculatePath() for %u\n", _sourceUnit->GetGUIDLow());

    BuildShortcut();
    _type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
    return true;
}

bool PathInfo::Update(float destX, float destY, float destZ, bool forceDest)
{
    float x, y, z;
    m_sourceUnit->GetPosition(x, y, z);

    if (!Oregon::IsValidMapCoord(destX, destY, destZ) || !Oregon::IsValidMapCoord(x, y, z))
    {
        sLog.outMMap("PathInfo::Update() called with invalid map coords, destX: %f destY: %f destZ: %f x: %f y: %f z: %f for creature %u", destX, destY, destZ, x, y, z, m_sourceUnit->GetGUIDLow());
        m_type = PATHFIND_NOPATH;
        return false;
    }

    Vector3 oldDest = getEndPosition();
    Vector3 newDest(destX, destY, destZ);
    setEndPosition(newDest);

    Vector3 newStart(x, y, z);
    setStartPosition(newStart);

    m_forceDestination = forceDest;

    sLog.outMMap("PathInfo::Update() for %u \n", m_sourceUnit->GetGUIDLow());

    // make sure navMesh works - we can run on map w/o mmap
    // check if the start and end point have a .mmtile loaded (can we pass via not loaded tile on the way?)
    if (!m_navMesh || !m_navMeshQuery || m_sourceUnit->HasUnitState(UNIT_STATE_IGNORE_PATHFINDING) ||
        !HaveTile(newStart) || !HaveTile(newDest))
    {
        BuildShortcut();
        m_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
        return true;
    }

    updateFilter();

    BuildPolyPath(newStart, newDest);
    return true;
}

////////////////// PathInfo //////////////////
PathInfo::PathInfo(const Unit* owner, const float destX, const float destY, const float destZ, bool useStraightPath) : m_pathPolyRefs(NULL),
    m_polyLength(0), m_type(PATHFIND_BLANK), m_useStraightPath(useStraightPath), m_sourceUnit(owner),
    m_navMesh(NULL), m_navMeshQuery(NULL)
{
    PathNode endPoint(destX, destY, destZ);
    setEndPosition(endPoint);

    float x,y,z;
    m_sourceUnit->GetPosition(x, y, z);
    PathNode startPoint(x, y, z);
    setStartPosition(startPoint);

    PATH_DEBUG("++ PathInfo::PathInfo for %u \n", m_sourceUnit->GetGUID());

    const Map* map = m_sourceUnit->GetMap();
    if (map->IsPathfindingEnabled())
        m_navMesh = map->GetNavMesh();

    if (m_navMesh)
    {
        m_navMeshQuery = dtAllocNavMeshQuery();
        ASSERT(m_navMeshQuery);
        if(DT_SUCCESS != m_navMeshQuery->init(m_navMesh, MESH_MAX_NODES))
        {
            sLog->outError("%u's PathInfo navMeshQuery failed to init", m_sourceUnit->GetGUID());
            return;
        }

        BuildPolyPath(startPoint, endPoint);
    }
    else
    {
        BuildShortcut();
        m_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
    }
}

//.........这里部分代码省略.........
                m_pathPolyRefs,     // current path
                m_polyLength,       // lenth of current path
                pathPoints,         // [out] path corner points
                NULL,               // [out] flags
                NULL,               // [out] shortened path
                (int*)&pointCount,
                m_pointPathLimit);   // maximum number of points/polygons to use
    }
    else
    {
        dtResult = findSmoothPath(
                startPoint,         // start position
                endPoint,           // end position
                m_pathPolyRefs,     // current path
                m_polyLength,       // length of current path
                pathPoints,         // [out] path corner points
                (int*)&pointCount,
                m_pointPathLimit);    // maximum number of points
    }

    if (pointCount < 2 || dtStatusFailed(dtResult))
    {
        // only happens if pass bad data to findStraightPath or navmesh is broken
        // single point paths can be generated here
        // TODO : check the exact cases
        DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ PathFinder::BuildPointPath FAILED! path sized %d returned\n", pointCount);
        BuildShortcut();
        m_type = PATHFIND_NOPATH;
        return;
    }

    uint32 tempPointCounter = 2;
    uint8  cutLimit         = 0;

    PointsArray    tempPathPoints;
    tempPathPoints.resize(pointCount);

    for (uint32 i = 0; i < pointCount; ++i)
        tempPathPoints[i] = Vector3(pathPoints[i*VERTEX_SIZE+2], pathPoints[i*VERTEX_SIZE], pathPoints[i*VERTEX_SIZE+1]);

    // Optimize points
    for (uint32 i = 1; i < pointCount - 1; ++i)
    {
        G3D::Vector3 p  = tempPathPoints[i];     // Point        
        G3D::Vector3 p1 = tempPathPoints[i - 1]; // PrevPoint
        G3D::Vector3 p2 = tempPathPoints[i + 1]; // NextPoint

        float line = (p1.y - p2.y) * p.x + (p2.x - p1.x) * p.y + (p1.x * p2.y - p2.x * p1.y);

        if (fabs(line) < LINE_FAULT && cutLimit < SKIP_POINT_LIMIT)
        {
            tempPathPoints[i] = Vector3(0, 0, 0);
            ++cutLimit;
        }
        else
        {
            ++tempPointCounter;
            cutLimit = 0;
        }
    }

    m_pathPoints.resize(tempPointCounter);

    uint32 b = 0;
    for (uint32 i = 0; i < pointCount; ++i)
    {
        if (tempPathPoints[i] != Vector3(0, 0, 0))
        {
            m_pathPoints[b] = tempPathPoints[i];
            ++b;
        }
    }

    pointCount = tempPointCounter;

    // first point is always our current location - we need the next one
    setActualEndPosition(m_pathPoints[pointCount - 1]);

    // force the given destination, if needed
    if (m_forceDestination &&
        (!(m_type & PATHFIND_NORMAL) || !inRange(getEndPosition(), getActualEndPosition(), 1.0f, 1.0f)))
    {
        // we may want to keep partial subpath
        if (dist3DSqr(getActualEndPosition(), getEndPosition()) <
            0.3f * dist3DSqr(getStartPosition(), getEndPosition()))
        {
            setActualEndPosition(getEndPosition());
            m_pathPoints[m_pathPoints.size()-1] = getEndPosition();
        }
        else
        {
            setActualEndPosition(getEndPosition());
            BuildShortcut();
        }

        m_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
    }

    DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ PathFinder::BuildPointPath path type %d size %d poly-size %d\n", m_type, pointCount, m_polyLength);
}

void PathInfo::BuildPointPath(float *startPoint, float *endPoint)
{
    float pathPoints[MAX_POINT_PATH_LENGTH*VERTEX_SIZE];
    uint32 pointCount = 0;
    dtStatus dtResult = DT_FAILURE;
    bool usedOffmesh = false;
    if (m_useStraightPath)
    {
        dtResult = m_navMeshQuery->findStraightPath(
                startPoint,         // start position
                endPoint,           // end position
                m_pathPolyRefs,     // current path
                m_polyLength,       // lenth of current path
                pathPoints,         // [out] path corner points
                NULL,               // [out] flags
                NULL,               // [out] shortened path
                (int*)&pointCount,
                MAX_POINT_PATH_LENGTH);   // maximum number of points/polygons to use
    }
    else
    {
        dtResult = findSmoothPath(
                startPoint,         // start position
                endPoint,           // end position
                m_pathPolyRefs,     // current path
                m_polyLength,       // length of current path
                pathPoints,         // [out] path corner points
                (int*)&pointCount,
                usedOffmesh,
                MAX_POINT_PATH_LENGTH);    // maximum number of points
    }

    if (pointCount < 2 || dtResult != DT_SUCCESS)
    {
        // only happens if pass bad data to findStraightPath or navmesh is broken
        // single point paths can be generated here
        // TODO : check the exact cases
        DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ PathInfo::BuildPointPath FAILED! path sized %d returned\n", pointCount);
        BuildShortcut();
        m_type = PATHFIND_NOPATH;
        return;
    }

    // we need to flash our poly path to prevent it being used as subpath next cycle
    // in case of off mesh connection was used
    if(usedOffmesh)
        m_polyLength = 0;

    m_pathPoints.resize(pointCount);
    for (uint32 i = 0; i < pointCount; ++i)
        m_pathPoints.set(i, PathNode(pathPoints[i*VERTEX_SIZE+2], pathPoints[i*VERTEX_SIZE], pathPoints[i*VERTEX_SIZE+1]));

    // first point is always our current location - we need the next one
    setNextPosition(m_pathPoints[1]);
    setActualEndPosition(m_pathPoints[pointCount-1]);

    // force the given destination, if needed
    if(m_forceDestination &&
        (!(m_type & PATHFIND_NORMAL) || !inRange(getEndPosition(), getActualEndPosition(), 1.0f, 1.0f)))
    {
        setActualEndPosition(getEndPosition());
        BuildShortcut();
        m_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
        return;
    }

    DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ PathInfo::BuildPointPath path type %d size %d poly-size %d\n", m_type, pointCount, m_polyLength);
}

void PathInfo::BuildPolyPath(PathNode startPos, PathNode endPos)
{
    // *** getting start/end poly logic ***

    float distToStartPoly, distToEndPoly;
    float startPoint[VERTEX_SIZE] = {startPos.y, startPos.z, startPos.x};
    float endPoint[VERTEX_SIZE] = {endPos.y, endPos.z, endPos.x};

    dtPolyRef startPoly = getPolyByLocation(startPoint, &distToStartPoly);
    dtPolyRef endPoly = getPolyByLocation(endPoint, &distToEndPoly);

    // 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)
    {
        DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ BuildPolyPath :: (startPoly == 0 || endPoly == 0)\n");
        BuildShortcut();
        m_type = (m_sourceUnit->GetTypeId() == TYPEID_UNIT && ((Creature*)m_sourceUnit)->CanFly())
                    ? 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)
    {
        DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ BuildPolyPath :: farFromPoly distToStartPoly=%.3f distToEndPoly=%.3f\n", distToStartPoly, distToEndPoly);

        bool buildShotrcut = false;
        if (m_sourceUnit->GetTypeId() == TYPEID_UNIT)
        {
            Creature* owner = (Creature*)m_sourceUnit;

            PathNode p = (distToStartPoly > 7.0f) ? startPos : endPos;
            if (m_sourceUnit->GetTerrain()->IsUnderWater(p.x, p.y, p.z))
            {
                DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ BuildPolyPath :: underWater case\n");
                if (owner->CanSwim())
                    buildShotrcut = true;
            }
            else
            {
                DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ BuildPolyPath :: flying case\n");
                if (owner->CanFly())
                    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)
    {
        DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ BuildPolyPath :: (startPoly == endPoly)\n");

        BuildShortcut();

        m_pathPolyRefs[0] = startPoly;
        m_polyLength = 1;

        m_type = farFromPoly ? PATHFIND_INCOMPLETE : PATHFIND_NORMAL;
        DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ 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)
        {
            // here to carch few bugs
            MANGOS_ASSERT(m_pathPolyRefs[pathStartIndex] != INVALID_POLYREF);

//.........这里部分代码省略.........

void PathFinder::BuildPointPath(const float *startPoint, const float *endPoint)
{
    float pathPoints[MAX_POINT_PATH_LENGTH*VERTEX_SIZE];
    uint32 pointCount = 0;
    dtStatus dtResult = DT_FAILURE;
    if(m_useStraightPath)
    {
        dtResult = m_navMeshQuery->findStraightPath(
                startPoint,         // start position
                endPoint,           // end position
                m_pathPolyRefs,     // current path
                m_polyLength,       // lenth of current path
                pathPoints,         // [out] path corner points
                NULL,               // [out] flags
                NULL,               // [out] shortened path
                (int*)&pointCount,
                m_pointPathLimit);   // maximum number of points/polygons to use
    }
    else
    {
        dtResult = findSmoothPath(
                startPoint,         // start position
                endPoint,           // end position
                m_pathPolyRefs,     // current path
                m_polyLength,       // length of current path
                pathPoints,         // [out] path corner points
                (int*)&pointCount,
                m_pointPathLimit);    // maximum number of points
    }

    if(pointCount < 2 || dtResult != DT_SUCCESS)
    {
        // only happens if pass bad data to findStraightPath or navmesh is broken
        // single point paths can be generated here
        // TODO : check the exact cases
        sLog->outDebug(LOG_FILTER_PATHFINDING, "++ PathFinder::BuildPointPath FAILED! path sized %d returned\n", pointCount);
        BuildShortcut();
        m_type = PATHFIND_NOPATH;
        return;
    }

    m_pathPoints.resize(pointCount);
    for(uint32 i = 0; i < pointCount; ++i)
        m_pathPoints[i] = Vector3(pathPoints[i*VERTEX_SIZE+2], pathPoints[i*VERTEX_SIZE], pathPoints[i*VERTEX_SIZE+1]);

    // first point is always our current location - we need the next one
    setActualEndPosition(m_pathPoints[pointCount-1]);

    // force the given destination, if needed
    if(m_forceDestination &&
        (!(m_type & PATHFIND_NORMAL) || !inRange(getEndPosition(), getActualEndPosition(), 1.0f, 1.0f)))
    {
        // we may want to keep partial subpath
        if(dist3DSqr(getActualEndPosition(), getEndPosition()) <
            0.3f * dist3DSqr(getStartPosition(), getEndPosition()))
        {
            setActualEndPosition(getEndPosition());
            m_pathPoints[m_pathPoints.size()-1] = getEndPosition();
        }
        else
        {
            setActualEndPosition(getEndPosition());
            BuildShortcut();
        }

        m_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
    }

    sLog->outDebug(LOG_FILTER_PATHFINDING, "++ PathFinder::BuildPointPath path type %d size %d poly-size %d\n", m_type, pointCount, m_polyLength);
}

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;
//.........这里部分代码省略.........

void PathInfo::updateNextPosition()
{
    float x, y, z;

    getStartPosition(x, y, z);
    float startPos[3] = {y, z, x};
    getEndPosition(x, y, z);
    float endPos[3] = {y, z, x};

    float* pathPoints = new float[MAX_PATH_LENGTH*3];
    int pointCount = m_navMesh->findStraightPath(
        startPos,           // start position
        endPos,             // end position
        m_pathPolyRefs,     // current path
        m_length,           // lenth of current path
        pathPoints,         // [out] path corner points
        0,                  // [out] flags
        0,                  // [out] shortened path  PATHFIND TODO: see if this is usable (IE, doesn't leave gaps in path)
        MAX_PATH_LENGTH);   // maximum number of points/polygons to use

    // TODO: imitate PATHFIND_ITER code from RecastDemo so that terrain following is smoother

    if(pointCount == 0)
    {
        delete [] pathPoints;

        // only happens if pass bad data to findStraightPath or navmesh is broken
        sLog.outDebug("%u's UpdateNextPosition failed: 0 length path", m_sourceObject->GetGUID());
        shortcut();
        return;
    }

    int ix, iy, iz;
    if(pointCount == 1)
    {
        ix = 2; iy = 0; iz = 1;
    }
    else
    {
        ix = 5; iy = 3; iz = 4;
    }

    setNextPosition(pathPoints[ix], pathPoints[iy], pathPoints[iz]);

    // if startPosition == nextPosition, NPC will never advance to destination
    if(isSamePoint(m_startPosition, m_nextPosition) && !isSamePoint(m_nextPosition, m_endPosition))
        setNextPosition(pathPoints[ix + 3], pathPoints[iy + 3], pathPoints[iz + 3]);

    delete [] m_pathPoints;
    m_pathPoints = pathPoints;

    m_type = PathType(m_type | PATHFIND_NORMAL);

    // if end position from findStraightPath isn't the
    // place we want to go, there is likely no path
    endPos[0] = pathPoints[(pointCount-1)*3+2];
    endPos[1] = pathPoints[(pointCount-1)*3];
    endPos[2] = pathPoints[(pointCount-1)*3+1];
    if(!dtVequal(endPos, m_endPosition))
        m_type = PathType(m_type | PATHFIND_NOPATH);
}

void PathFinderMovementGenerator::BuildPointPath(const float *startPoint, const float *endPoint)
{
    float pathPoints[MAX_POINT_PATH_LENGTH*VERTEX_SIZE];
    uint32 pointCount = 0;
    dtStatus dtResult = DT_FAILURE;
    if (m_useStraightPath)
    {
        dtResult = m_navMeshQuery->findStraightPath(
                startPoint,         // start position
                endPoint,           // end position
                m_pathPolyRefs,     // current path
                m_polyLength,       // lenth of current path
                pathPoints,         // [out] path corner points
                NULL,               // [out] flags
                NULL,               // [out] shortened path
                (int*)&pointCount,
                m_pointPathLimit);   // maximum number of points/polygons to use
    }
    else
    {
        dtResult