bool nav_mesh::los(const float* start, const float* end, float* hitPos)
{
	float t = 0;
	float m_hitNormal[3] = { 0, };

	dtPolyRef startRef = 0;
	float polyPickExt[3] = { 2, 4, 2 };

	dtStatus status = query_->findNearestPoly(start, polyPickExt, &filter_, &startRef, nullptr);
	if (dtStatusFailed(status))
	{
		return false;
	}

	dtPolyRef polys[MAX_POLYS] = { 0, };
	int npolys = 0;

	query_->raycast(startRef, start, end, &filter_, &t, m_hitNormal, polys, &npolys, sizeof(polys) / sizeof(polys[0]));
	if (t > 1)
	{
		dtVcopy(hitPos, end);
		return true;
	}
	else
	{
		if (npolys > 0)
		{
			dtVlerp(hitPos, start, end, t);

			float temp[3] = { 0, };
			dtStatus statu = query_->closestPointOnPoly(polys[npolys - 1], hitPos, temp, nullptr);
			if (dtStatusSucceed(statu))
			{
				dtVcopy(hitPos, temp);
				return false;
			}
		}
	}

	dtVcopy(hitPos, start);
	return false;
}

JNIEXPORT jint JNICALL Java_ru_mmocore_external_recastnavigation_detour_NavMeshQuery_queryPolygons
  (JNIEnv *env, jobject, jlong context, jfloatArray center, jfloatArray extents, jlong filter, jintArray polys, jintArray polyCount)
{
	NATIVE;

	TO_FLOAT_ARRAY(center, 3);
	TO_FLOAT_ARRAY(extents, 3);

	POLYREF_ARRAY(polys);

	int m_polyCount = 0;

	dtStatus status = d->queryPolygons(m_center, m_extents, qFilter, m_polys, &m_polyCount, polysSize);
	if (dtStatusSucceed(status)) {
		SET_INT_ARRAY(polys);
		SET_INT_ARRAY_ONE(polyCount);
	}

	return status;
}

JNIEXPORT jint JNICALL Java_ru_mmocore_external_recastnavigation_detour_NavMeshQuery_getPolyWallSegments
  (JNIEnv *env, jobject, jlong context, jint ref, jlong filter, jfloatArray segmentVerts, jintArray segmentRefs, jintArray segmentCount)
{
	NATIVE;

	FLOAT_ARRAY(segmentVerts);

	POLYREF_ARRAY(segmentRefs);

	int m_segmentCount;

	dtStatus status = d->getPolyWallSegments(ref, qFilter, m_segmentVerts, m_segmentRefs, &m_segmentCount, segmentRefsSize);

	if (dtStatusSucceed(status)) {
		SET_FLOAT_ARRAY(segmentVerts);
		SET_INT_ARRAY(segmentRefs);
		SET_INT_ARRAY_ONE(segmentCount);
	}

	return status;
}

bool dtPathCorridor::canMoveOverOffmeshConnection(dtPolyRef offMeshConRef, dtPolyRef* refs,
	const float* agentPos, float* startPos, float* endPos,
	dtNavMeshQuery* navquery) const
{
	dtAssert(navquery);
	dtAssert(m_path);
	dtAssert(m_npath);

	// Advance the path up to and over the off-mesh connection.
	dtPolyRef prevRef = 0, polyRef = m_path[0];
	int npos = 0;
	while (npos < m_npath && polyRef != offMeshConRef)
	{
		prevRef = polyRef;
		polyRef = m_path[npos];
		npos++;
	}
	if (npos == m_npath)
	{
		// Could not find offMeshConRef
		return false;
	}

	refs[0] = prevRef;
	refs[1] = polyRef;

	const dtNavMesh* nav = navquery->getAttachedNavMesh();
	dtAssert(nav);

	dtStatus status = nav->getOffMeshConnectionPolyEndPoints(refs[0], refs[1], agentPos, startPos, endPos);
	if (dtStatusSucceed(status))
	{
		return true;
	}

	return true;
}

/**
@par

Behavior:

- The movement is constrained to the surface of the navigation mesh.
- The corridor is automatically adjusted (shorted or lengthened) in order to remain valid.
- The new position will be located in the adjusted corridor's first polygon.

The expected use case is that the desired position will be 'near' the current corridor. What is considered 'near'
depends on local polygon density, query search extents, etc.

The resulting position will differ from the desired position if the desired position is not on the navigation mesh,
or it can't be reached using a local search.
*/
bool dtPathCorridor::movePosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter)
{
    dtAssert(m_path);
    dtAssert(m_npath);

    // Move along navmesh and update new position.
    float result[3];
    static const int MAX_VISITED = 16;
    dtPolyRef visited[MAX_VISITED];
    int nvisited = 0;
    dtStatus status = navquery->moveAlongSurface(m_path[0], m_pos, npos, filter,
                      result, visited, &nvisited, MAX_VISITED);
    if (dtStatusSucceed(status)) {
        m_npath = dtMergeCorridorStartMoved(m_path, m_npath, m_maxPath, visited, nvisited);

        // Adjust the position to stay on top of the navmesh.
        float h = m_pos[1];
        navquery->getPolyHeight(m_path[0], result, &h);
        result[1] = h;
        dtVcopy(m_pos, result);
        return true;
    }
    return false;
}

void PathGenerator::BuildPolyPath(G3D::Vector3 const& startPos, G3D::Vector3 const& 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 and swimming exception )
    // its up to caller how he will use this info
    if (startPoly == INVALID_POLYREF || endPoly == INVALID_POLYREF)
    {
        TC_LOG_DEBUG("maps", "++ BuildPolyPath :: (startPoly == 0 || endPoly == 0)\n");
        BuildShortcut();
        bool path = _sourceUnit->GetTypeId() == TYPEID_UNIT && _sourceUnit->ToCreature()->CanFly();

        bool waterPath = _sourceUnit->GetTypeId() == TYPEID_UNIT && _sourceUnit->ToCreature()->CanSwim();
        if (waterPath)
        {
            // Check both start and end points, if they're both in water, then we can *safely* let the creature move
            for (uint32 i = 0; i < _pathPoints.size(); ++i)
            {
                ZLiquidStatus status = _sourceUnit->GetBaseMap()->getLiquidStatus(_pathPoints[i].x, _pathPoints[i].y, _pathPoints[i].z, MAP_ALL_LIQUIDS, NULL);
                // One of the points is not in the water, cancel movement.
                if (status == LIQUID_MAP_NO_WATER)
                {
                    waterPath = false;
                    break;
                }
            }
        }

        _type = (path || waterPath) ? 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)
    {
        TC_LOG_DEBUG("maps", "++ BuildPolyPath :: farFromPoly distToStartPoly=%.3f distToEndPoly=%.3f\n", distToStartPoly, distToEndPoly);

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

            G3D::Vector3 const& p = (distToStartPoly > 7.0f) ? startPos : endPos;
            if (_sourceUnit->GetBaseMap()->IsUnderWater(p.x, p.y, p.z))
            {
                TC_LOG_DEBUG("maps", "++ BuildPolyPath :: underWater case\n");
                if (owner->CanSwim())
                    buildShotrcut = true;
            }
            else
            {
                TC_LOG_DEBUG("maps", "++ BuildPolyPath :: flying case\n");
                if (owner->CanFly())
                    buildShotrcut = true;
            }
        }

        if (buildShotrcut)
        {
            BuildShortcut();
            _type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
            return;
        }
        else
        {
            float closestPoint[VERTEX_SIZE];
            // we may want to use closestPointOnPolyBoundary instead
            if (dtStatusSucceed(_navMeshQuery->closestPointOnPoly(endPoly, endPoint, closestPoint, NULL)))
            {
                dtVcopy(endPoint, closestPoint);
                SetActualEndPosition(G3D::Vector3(endPoint[2], endPoint[0], endPoint[1]));
            }

            _type = PATHFIND_INCOMPLETE;
        }
    }

    // *** poly path generating logic ***

    // start and end are on same polygon
    // just need to move in straight line
    if (startPoly == endPoly)
    {
        TC_LOG_DEBUG("maps", "++ BuildPolyPath :: (startPoly == endPoly)\n");

        BuildShortcut();

        _pathPolyRefs[0] = startPoly;
        _polyLength = 1;

        _type = farFromPoly ? PATHFIND_INCOMPLETE : PATHFIND_NORMAL;
//.........这里部分代码省略.........

void dtCrowd::updateMoveRequest(const float /*dt*/)
{
	// Fire off new requests.
	for (int i = 0; i < m_moveRequestCount; ++i)
	{
		MoveRequest* req = &m_moveRequests[i];
		dtCrowdAgent* ag = &m_agents[req->idx];
		
		// Agent not active anymore, kill request.
		if (!ag->active)
			req->state = MR_TARGET_FAILED;
		
		// Adjust target
		if (req->aref)
		{
			if (req->state == MR_TARGET_ADJUST)
			{
				// Adjust existing path.
				ag->corridor.moveTargetPosition(req->apos, m_navquery, &m_filter);
				req->state = MR_TARGET_VALID;
			}
			else
			{
				// Adjust on the flight request.
				float result[3];
				static const int MAX_VISITED = 16;
				dtPolyRef visited[MAX_VISITED];
				int nvisited = 0;
				m_navquery->moveAlongSurface(req->temp[req->ntemp-1], req->pos, req->apos, &m_filter,
											 result, visited, &nvisited, MAX_VISITED);
				req->ntemp = dtMergeCorridorEndMoved(req->temp, req->ntemp, MAX_TEMP_PATH, visited, nvisited);
				dtVcopy(req->pos, result);
				
				// Reset adjustment.
				dtVset(req->apos, 0,0,0);
				req->aref = 0;
			}
		}
		
		
		if (req->state == MR_TARGET_REQUESTING)
		{
			// Calculate request position.
			// If there is a lot of latency between requests, it is possible to
			// project the current position ahead and use raycast to find the actual
			// location and path.
			const dtPolyRef* path = ag->corridor.getPath();
			const int npath = ag->corridor.getPathCount();
			dtAssert(npath);
			
			// Here we take the simple approach and set the path to be just the current location.
			float reqPos[3];
			dtVcopy(reqPos, ag->corridor.getPos());	// The location of the request
			dtPolyRef reqPath[8];					// The path to the request location
			reqPath[0] = path[0];
			int reqPathCount = 1;
			
			req->pathqRef = m_pathq.request(reqPath[reqPathCount-1], req->ref, reqPos, req->pos, &m_filter);
			if (req->pathqRef != DT_PATHQ_INVALID)
			{
				ag->corridor.setCorridor(reqPos, reqPath, reqPathCount);
				req->state = MR_TARGET_WAITING_FOR_PATH;
			}
		}
	}

	
	// Update requests.
	m_pathq.update(MAX_ITERS_PER_UPDATE);

	// Process path results.
	for (int i = 0; i < m_moveRequestCount; ++i)
	{
		MoveRequest* req = &m_moveRequests[i];
		dtCrowdAgent* ag = &m_agents[req->idx];
		
		if (req->state == MR_TARGET_WAITING_FOR_PATH)
		{
			// Poll path queue.
			dtStatus status = m_pathq.getRequestStatus(req->pathqRef);
			if (dtStatusFailed(status))
			{
				req->pathqRef = DT_PATHQ_INVALID;
				req->state = MR_TARGET_FAILED;
			}
			else if (dtStatusSucceed(status))
			{
				const dtPolyRef* path = ag->corridor.getPath();
				const int npath = ag->corridor.getPathCount();
				dtAssert(npath);
				
				// Apply results.
				float targetPos[3];
				dtVcopy(targetPos, req->pos);
				
				dtPolyRef* res = m_pathResult;
				bool valid = true;
				int nres = 0;
				dtStatus status = m_pathq.getPathResult(req->pathqRef, res, &nres, m_maxPathResult);
				if (dtStatusFailed(status) || !nres)
//.........这里部分代码省略.........

//.........这里部分代码省略.........
            bool offMeshConnection = (steerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ? true : false;

            // Find movement delta.
            float delta[3], len;
            dtVsub(delta, steerPos, iterPos);
            len = dtMathSqrtf(dtVdot(delta, delta));
            // If the steer target is end of path or off-mesh link, do not move past the location.
            if ((endOfPath || offMeshConnection) && len < STEP_SIZE)
                len = 1;
            else
                len = STEP_SIZE / len;
            float moveTgt[3];
            dtVmad(moveTgt, iterPos, delta, len);

            // Move
            float result[3];
            dtPolyRef visited[16];
            int nvisited = 0;
            _navMeshQuery->moveAlongSurface(polys[0], iterPos, moveTgt, &filter,
                result, visited, &nvisited, 16);

            npolys = fixupCorridor(polys, npolys, MAX_POLYS, visited, nvisited);
            npolys = fixupShortcuts(polys, npolys, _navMeshQuery);

            float h = 0;
            _navMeshQuery->getPolyHeight(polys[0], result, &h);
            result[1] = h;
            dtVcopy(iterPos, result);

            // Handle end of path and off-mesh links when close enough.
            if (endOfPath && inRange(iterPos, steerPos, SLOP, 1.0f))
            {
                // Reached end of path.
                dtVcopy(iterPos, targetPos);
                if (nsmoothPath < MAX_SMOOTH)
                {
                    //dtVcopy(&m_smoothPath[m_nsmoothPath * 3], iterPos);
                    //m_nsmoothPath++;
                    pathPoints.push_back(Vec3(iterPos[0], iterPos[1], iterPos[2]));
                    nsmoothPath++;
                }
                break;
            }
            else if (offMeshConnection && inRange(iterPos, steerPos, SLOP, 1.0f))
            {
                // Reached off-mesh connection.
                float startPos[3], endPos[3];

                // Advance the path up to and over the off-mesh connection.
                dtPolyRef prevRef = 0, polyRef = polys[0];
                int npos = 0;
                while (npos < npolys && polyRef != steerPosRef)
                {
                    prevRef = polyRef;
                    polyRef = polys[npos];
                    npos++;
                }
                for (int i = npos; i < npolys; ++i)
                    polys[i - npos] = polys[i];
                npolys -= npos;

                // Handle the connection.
                dtStatus status = _navMesh->getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos);
                if (dtStatusSucceed(status))
                {
                    if (nsmoothPath < MAX_SMOOTH)
                    {
                        //dtVcopy(&m_smoothPath[m_nsmoothPath * 3], startPos);
                        //m_nsmoothPath++;
                        pathPoints.push_back(Vec3(startPos[0], startPos[1], startPos[2]));
                        nsmoothPath++;
                        // Hack to make the dotted path not visible during off-mesh connection.
                        if (nsmoothPath & 1)
                        {
                            //dtVcopy(&m_smoothPath[m_nsmoothPath * 3], startPos);
                            //m_nsmoothPath++;
                            pathPoints.push_back(Vec3(startPos[0], startPos[1], startPos[2]));
                            nsmoothPath++;
                        }
                    }
                    // Move position at the other side of the off-mesh link.
                    dtVcopy(iterPos, endPos);
                    float eh = 0.0f;
                    _navMeshQuery->getPolyHeight(polys[0], iterPos, &eh);
                    iterPos[1] = eh;
                }
            }

            // Store results.
            if (nsmoothPath < MAX_SMOOTH)
            {
                //dtVcopy(&m_smoothPath[m_nsmoothPath * 3], iterPos);
                //m_nsmoothPath++;

                pathPoints.push_back(Vec3(iterPos[0], iterPos[1], iterPos[2]));
                nsmoothPath++;
            }
        }
    }
}

void NavMeshTesterTool::handleMenu()
{
	if (imguiCheck("Pathfind Follow", m_toolMode == TOOLMODE_PATHFIND_FOLLOW))
	{
		m_toolMode = TOOLMODE_PATHFIND_FOLLOW;
		recalc();
	}
	if (imguiCheck("Pathfind Straight", m_toolMode == TOOLMODE_PATHFIND_STRAIGHT))
	{
		m_toolMode = TOOLMODE_PATHFIND_STRAIGHT;
		recalc();
	}
	if (m_toolMode == TOOLMODE_PATHFIND_STRAIGHT)
	{
		imguiIndent();
		imguiLabel("Vertices at crossings");
		if (imguiCheck("None", m_straightPathOptions == 0))
		{
			m_straightPathOptions = 0;
			recalc();
		}
		if (imguiCheck("Area", m_straightPathOptions == DT_STRAIGHTPATH_AREA_CROSSINGS))
		{
			m_straightPathOptions = DT_STRAIGHTPATH_AREA_CROSSINGS;
			recalc();
		}
		if (imguiCheck("All", m_straightPathOptions == DT_STRAIGHTPATH_ALL_CROSSINGS))
		{
			m_straightPathOptions = DT_STRAIGHTPATH_ALL_CROSSINGS;
			recalc();
		}

		imguiUnindent();
	}
	if (imguiCheck("Pathfind Sliced", m_toolMode == TOOLMODE_PATHFIND_SLICED))
	{
		m_toolMode = TOOLMODE_PATHFIND_SLICED;
		recalc();
	}

	imguiSeparator();

	if (imguiCheck("Distance to Wall", m_toolMode == TOOLMODE_DISTANCE_TO_WALL))
	{
		m_toolMode = TOOLMODE_DISTANCE_TO_WALL;
		recalc();
	}

	imguiSeparator();

	if (imguiCheck("Raycast", m_toolMode == TOOLMODE_RAYCAST))
	{
		m_toolMode = TOOLMODE_RAYCAST;
		recalc();
	}

	imguiSeparator();

	if (imguiCheck("Find Polys in Circle", m_toolMode == TOOLMODE_FIND_POLYS_IN_CIRCLE))
	{
		m_toolMode = TOOLMODE_FIND_POLYS_IN_CIRCLE;
		recalc();
	}
	if (imguiCheck("Find Polys in Shape", m_toolMode == TOOLMODE_FIND_POLYS_IN_SHAPE))
	{
		m_toolMode = TOOLMODE_FIND_POLYS_IN_SHAPE;
		recalc();
	}

	imguiSeparator();

	if (imguiCheck("Find Local Neighbourhood", m_toolMode == TOOLMODE_FIND_LOCAL_NEIGHBOURHOOD))
	{
		m_toolMode = TOOLMODE_FIND_LOCAL_NEIGHBOURHOOD;
		recalc();
	}

	imguiSeparator();
	
	if (imguiButton("Set Random Start"))
	{
		dtStatus status = m_navQuery->findRandomPoint(&m_filter, frand, &m_startRef, m_spos);
		if (dtStatusSucceed(status))
		{
			m_sposSet = true;
			recalc();
		}
	}
	if (imguiButton("Set Random End", m_sposSet))
	{
		if (m_sposSet)
		{
			dtStatus status = m_navQuery->findRandomPointAroundCircle(m_startRef, m_spos, m_randomRadius, &m_filter, frand, &m_endRef, m_epos);
			if (dtStatusSucceed(status))
			{
				m_eposSet = true;
				recalc();
			}
		}
	}
//.........这里部分代码省略.........

dtStatus PathFinder::findSmoothPath(const float* startPos, const float* endPos,
                                    const dtPolyRef* polyPath, uint32 polyPathSize,
                                    float* smoothPath, int* smoothPathSize, uint32 maxSmoothPathSize)
{
    *smoothPathSize = 0;
    uint32 nsmoothPath = 0;

    dtPolyRef polys[MAX_PATH_LENGTH];
    memcpy(polys, polyPath, sizeof(dtPolyRef)*polyPathSize);
    uint32 npolys = polyPathSize;

    float iterPos[VERTEX_SIZE], targetPos[VERTEX_SIZE];
    if (dtStatusFailed(m_navMeshQuery->closestPointOnPolyBoundary(polys[0], startPos, iterPos)))
        { return DT_FAILURE; }

    if (dtStatusFailed(m_navMeshQuery->closestPointOnPolyBoundary(polys[npolys - 1], endPos, targetPos)))
        { return DT_FAILURE; }

    dtVcopy(&smoothPath[nsmoothPath * VERTEX_SIZE], iterPos);
    ++nsmoothPath;

    // Move towards target a small advancement at a time until target reached or
    // when ran out of memory to store the path.
    while (npolys && nsmoothPath < maxSmoothPathSize)
    {
        // Find location to steer towards.
        float steerPos[VERTEX_SIZE];
        unsigned char steerPosFlag;
        dtPolyRef steerPosRef = INVALID_POLYREF;

        if (!getSteerTarget(iterPos, targetPos, SMOOTH_PATH_SLOP, polys, npolys, steerPos, steerPosFlag, steerPosRef))
            { break; }

        bool endOfPath = (steerPosFlag & DT_STRAIGHTPATH_END);
        bool offMeshConnection = (steerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION);

        // Find movement delta.
        float delta[VERTEX_SIZE];
        dtVsub(delta, steerPos, iterPos);
        float len = dtSqrt(dtVdot(delta, delta));
        // If the steer target is end of path or off-mesh link, do not move past the location.
        if ((endOfPath || offMeshConnection) && len < SMOOTH_PATH_STEP_SIZE)
            { len = 1.0f; }
        else
            { len = SMOOTH_PATH_STEP_SIZE / len; }

        float moveTgt[VERTEX_SIZE];
        dtVmad(moveTgt, iterPos, delta, len);

        // Move
        float result[VERTEX_SIZE];
        const static uint32 MAX_VISIT_POLY = 16;
        dtPolyRef visited[MAX_VISIT_POLY];

        uint32 nvisited = 0;
        m_navMeshQuery->moveAlongSurface(polys[0], iterPos, moveTgt, &m_filter, result, visited, (int*)&nvisited, MAX_VISIT_POLY);
        npolys = fixupCorridor(polys, npolys, MAX_PATH_LENGTH, visited, nvisited);

        m_navMeshQuery->getPolyHeight(polys[0], result, &result[1]);
        result[1] += 0.5f;
        dtVcopy(iterPos, result);

        // Handle end of path and off-mesh links when close enough.
        if (endOfPath && inRangeYZX(iterPos, steerPos, SMOOTH_PATH_SLOP, 1.0f))
        {
            // Reached end of path.
            dtVcopy(iterPos, targetPos);
            if (nsmoothPath < maxSmoothPathSize)
            {
                dtVcopy(&smoothPath[nsmoothPath * VERTEX_SIZE], iterPos);
                ++nsmoothPath;
            }
            break;
        }
        else if (offMeshConnection && inRangeYZX(iterPos, steerPos, SMOOTH_PATH_SLOP, 1.0f))
        {
            // Advance the path up to and over the off-mesh connection.
            dtPolyRef prevRef = INVALID_POLYREF;
            dtPolyRef polyRef = polys[0];
            uint32 npos = 0;
            while (npos < npolys && polyRef != steerPosRef)
            {
                prevRef = polyRef;
                polyRef = polys[npos];
                ++npos;
            }

            for (uint32 i = npos; i < npolys; ++i)
                { polys[i - npos] = polys[i]; }

            npolys -= npos;

            // Handle the connection.
            float startPos[VERTEX_SIZE], endPos[VERTEX_SIZE];
            if (dtStatusSucceed(m_navMesh->getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos)))
            {
                if (nsmoothPath < maxSmoothPathSize)
                {
                    dtVcopy(&smoothPath[nsmoothPath * VERTEX_SIZE], startPos);
                    ++nsmoothPath;
//.........这里部分代码省略.........

//.........这里部分代码省略.........
		if (ag->targetPathqRef != DT_PATHQ_INVALID)
			ag->targetState = DT_CROWDAGENT_TARGET_WAITING_FOR_PATH;
	}

	
	// Update requests.
	m_pathq.update(MAX_ITERS_PER_UPDATE);

	dtStatus status;

	// Process path results.
	for (int i = 0; i < m_maxAgents; ++i)
	{
		dtCrowdAgent* ag = &m_agents[i];
		if (!ag->active)
			continue;
		if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
			continue;
		
		if (ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_PATH)
		{
			// Poll path queue.
			status = m_pathq.getRequestStatus(ag->targetPathqRef);
			if (dtStatusFailed(status))
			{
				// Path find failed, retry if the target location is still valid.
				ag->targetPathqRef = DT_PATHQ_INVALID;
				if (ag->targetRef)
					ag->targetState = DT_CROWDAGENT_TARGET_REQUESTING;
				else
					ag->targetState = DT_CROWDAGENT_TARGET_FAILED;
				ag->targetReplanTime = 0.0;
			}
			else if (dtStatusSucceed(status))
			{
				const dtPolyRef* path = ag->corridor.getPath();
				const int npath = ag->corridor.getPathCount();
				dtAssert(npath);
				
				// Apply results.
				float targetPos[3];
				dtVcopy(targetPos, ag->targetPos);
				
				dtPolyRef* res = m_pathResult;
				bool valid = true;
				int nres = 0;
				status = m_pathq.getPathResult(ag->targetPathqRef, res, &nres, m_maxPathResult);
				if (dtStatusFailed(status) || !nres)
					valid = false;
				
				// Merge result and existing path.
				// The agent might have moved whilst the request is
				// being processed, so the path may have changed.
				// We assume that the end of the path is at the same location
				// where the request was issued.
				
				// The last ref in the old path should be the same as
				// the location where the request was issued..
				if (valid && path[npath-1] != res[0])
					valid = false;
				
				if (valid)
				{
					// Put the old path infront of the old path.
					if (npath > 1)
					{

//.........这里部分代码省略.........
	unsigned char steerPosFlag;
	dtPolyRef steerPosRef;
		
	if (!getSteerTarget(m_navQuery, m_iterPos, m_targetPos, SLOP,
						m_pathIterPolys, m_pathIterPolyCount, steerPos, steerPosFlag, steerPosRef,
						m_steerPoints, &m_steerPointCount))
		return;
		
	dtVcopy(m_steerPos, steerPos);
	
	bool endOfPath = (steerPosFlag & DT_STRAIGHTPATH_END) ? true : false;
	bool offMeshConnection = (steerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ? true : false;
		
	// Find movement delta.
	float delta[3], len;
	dtVsub(delta, steerPos, m_iterPos);
	len = sqrtf(dtVdot(delta,delta));
	// If the steer target is end of path or off-mesh link, do not move past the location.
	if ((endOfPath || offMeshConnection) && len < STEP_SIZE)
		len = 1;
	else
		len = STEP_SIZE / len;
	float moveTgt[3];
	dtVmad(moveTgt, m_iterPos, delta, len);
		
	// Move
	float result[3];
	dtPolyRef visited[16];
	int nvisited = 0;
	m_navQuery->moveAlongSurface(m_pathIterPolys[0], m_iterPos, moveTgt, &m_filter,
								 result, visited, &nvisited, 16);
	m_pathIterPolyCount = fixupCorridor(m_pathIterPolys, m_pathIterPolyCount, MAX_POLYS, visited, nvisited);
	m_pathIterPolyCount = fixupShortcuts(m_pathIterPolys, m_pathIterPolyCount, m_navQuery);

	float h = 0;
	m_navQuery->getPolyHeight(m_pathIterPolys[0], result, &h);
	result[1] = h;
	dtVcopy(m_iterPos, result);
	
	// Handle end of path and off-mesh links when close enough.
	if (endOfPath && inRange(m_iterPos, steerPos, SLOP, 1.0f))
	{
		// Reached end of path.
		dtVcopy(m_iterPos, m_targetPos);
		if (m_nsmoothPath < MAX_SMOOTH)
		{
			dtVcopy(&m_smoothPath[m_nsmoothPath*3], m_iterPos);
			m_nsmoothPath++;
		}
		return;
	}
	else if (offMeshConnection && inRange(m_iterPos, steerPos, SLOP, 1.0f))
	{
		// Reached off-mesh connection.
		float startPos[3], endPos[3];
		
		// Advance the path up to and over the off-mesh connection.
		dtPolyRef prevRef = 0, polyRef = m_pathIterPolys[0];
		int npos = 0;
		while (npos < m_pathIterPolyCount && polyRef != steerPosRef)
		{
			prevRef = polyRef;
			polyRef = m_pathIterPolys[npos];
			npos++;
		}
		for (int i = npos; i < m_pathIterPolyCount; ++i)
			m_pathIterPolys[i-npos] = m_pathIterPolys[i];
		m_pathIterPolyCount -= npos;
				
		// Handle the connection.
		dtStatus status = m_navMesh->getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos);
		if (dtStatusSucceed(status))
		{
			if (m_nsmoothPath < MAX_SMOOTH)
			{
				dtVcopy(&m_smoothPath[m_nsmoothPath*3], startPos);
				m_nsmoothPath++;
				// Hack to make the dotted path not visible during off-mesh connection.
				if (m_nsmoothPath & 1)
				{
					dtVcopy(&m_smoothPath[m_nsmoothPath*3], startPos);
					m_nsmoothPath++;
				}
			}
			// Move position at the other side of the off-mesh link.
			dtVcopy(m_iterPos, endPos);
			float eh = 0.0f;
			m_navQuery->getPolyHeight(m_pathIterPolys[0], m_iterPos, &eh);
			m_iterPos[1] = eh;
		}
	}
	
	// Store results.
	if (m_nsmoothPath < MAX_SMOOTH)
	{
		dtVcopy(&m_smoothPath[m_nsmoothPath*3], m_iterPos);
		m_nsmoothPath++;
	}

}

//-------------------------------------------------------------------------------------
int NavMeshHandle::findRandomPointAroundCircle(int layer, const Position3D& centerPos, 
	std::vector<Position3D>& points, uint32 max_points, float maxRadius)
{
	std::map<int, NavmeshLayer>::iterator iter = navmeshLayer.find(layer);
	if(iter == navmeshLayer.end())
	{
		ERROR_MSG(fmt::format("NavMeshHandle::findRandomPointAroundCircle: not found layer({})\n",  layer));
		return NAV_ERROR;
	}

	dtNavMeshQuery* navmeshQuery = iter->second.pNavmeshQuery;
	
	dtQueryFilter filter;
	filter.setIncludeFlags(0xffff);
	filter.setExcludeFlags(0);

	if (maxRadius <= 0.0001f)
	{
		Position3D currpos;
		
		for (uint32 i = 0; i < max_points; i++)
		{
			float pt[3];
			dtPolyRef ref;
			dtStatus status = navmeshQuery->findRandomPoint(&filter, frand, &ref, pt);
			if (dtStatusSucceed(status))
			{
				currpos.x = pt[0];
				currpos.y = pt[1];
				currpos.z = pt[2];

				points.push_back(currpos);
			}
		}

		return (int)points.size();
	}
	
	const float extents[3] = {2.f, 4.f, 2.f};

	dtPolyRef startRef = INVALID_NAVMESH_POLYREF;

	float spos[3];
	spos[0] = centerPos.x;
	spos[1] = centerPos.y;
	spos[2] = centerPos.z;

	float startNearestPt[3];
	navmeshQuery->findNearestPoly(spos, extents, &filter, &startRef, startNearestPt);

	if (!startRef)
	{
		ERROR_MSG(fmt::format("NavMeshHandle::findRandomPointAroundCircle({1}): Could not find any nearby poly's ({0})\n", startRef, resPath));
		return NAV_ERROR_NEARESTPOLY;
	}
	
	Position3D currpos;
	bool done = false;
	int itry = 0;

	while (itry++ < 3 && points.size() == 0)
	{
		max_points -= points.size();

		for (uint32 i = 0; i < max_points; i++)
		{
			float pt[3];
			dtPolyRef ref;
			dtStatus status = navmeshQuery->findRandomPointAroundCircle(startRef, spos, maxRadius, &filter, frand, &ref, pt);

			if (dtStatusSucceed(status))
			{
				done = true;
				currpos.x = pt[0];
				currpos.y = pt[1];
				currpos.z = pt[2];

				Position3D v = centerPos - currpos;
				float dist_len = KBEVec3Length(&v);
				if (dist_len > maxRadius)
					continue;

				points.push_back(currpos);
			}
		}

		if (!done)
			break;
	}

	return (int)points.size();
}

    UpdateNavMeshStatus updateNavMesh(const osg::Vec3f& agentHalfExtents, const RecastMesh* recastMesh,
        const TilePosition& changedTile, const TilePosition& playerTile,
        const std::vector<OffMeshConnection>& offMeshConnections, const Settings& settings,
        const SharedNavMeshCacheItem& navMeshCacheItem, NavMeshTilesCache& navMeshTilesCache)
    {
        log("update NavMesh with mutiple tiles:",
            " agentHeight=", std::setprecision(std::numeric_limits<float>::max_exponent10),
            getHeight(settings, agentHalfExtents),
            " agentMaxClimb=", std::setprecision(std::numeric_limits<float>::max_exponent10),
            getMaxClimb(settings),
            " agentRadius=", std::setprecision(std::numeric_limits<float>::max_exponent10),
            getRadius(settings, agentHalfExtents),
            " changedTile=", changedTile,
            " playerTile=", playerTile,
            " changedTileDistance=", getDistance(changedTile, playerTile));

        const auto params = *navMeshCacheItem.lockConst()->getValue().getParams();
        const osg::Vec3f origin(params.orig[0], params.orig[1], params.orig[2]);

        const auto x = changedTile.x();
        const auto y = changedTile.y();

        const auto removeTile = [&] {
            const auto locked = navMeshCacheItem.lock();
            auto& navMesh = locked->getValue();
            const auto tileRef = navMesh.getTileRefAt(x, y, 0);
            const auto removed = dtStatusSucceed(navMesh.removeTile(tileRef, nullptr, nullptr));
            if (removed)
                locked->removeUsedTile(changedTile);
            return makeUpdateNavMeshStatus(removed, false);
        };

        if (!recastMesh)
        {
            log("ignore add tile: recastMesh is null");
            return removeTile();
        }

        auto recastMeshBounds = recastMesh->getBounds();

        for (const auto& water : recastMesh->getWater())
        {
            const auto waterBounds = getWaterBounds(water, settings, agentHalfExtents);
            recastMeshBounds.mMin.y() = std::min(recastMeshBounds.mMin.y(), waterBounds.mMin.y());
            recastMeshBounds.mMax.y() = std::max(recastMeshBounds.mMax.y(), waterBounds.mMax.y());
        }

        if (isEmpty(recastMeshBounds))
        {
            log("ignore add tile: recastMesh is empty");
            return removeTile();
        }

        if (!shouldAddTile(changedTile, playerTile, params.maxTiles))
        {
            log("ignore add tile: too far from player");
            return removeTile();
        }

        auto cachedNavMeshData = navMeshTilesCache.get(agentHalfExtents, changedTile, *recastMesh, offMeshConnections);

        if (!cachedNavMeshData)
        {
            const auto tileBounds = makeTileBounds(settings, changedTile);
            const osg::Vec3f tileBorderMin(tileBounds.mMin.x(), recastMeshBounds.mMin.y() - 1, tileBounds.mMin.y());
            const osg::Vec3f tileBorderMax(tileBounds.mMax.x(), recastMeshBounds.mMax.y() + 1, tileBounds.mMax.y());

            auto navMeshData = makeNavMeshTileData(agentHalfExtents, *recastMesh, offMeshConnections, changedTile,
                tileBorderMin, tileBorderMax, settings);

            if (!navMeshData.mValue)
            {
                log("ignore add tile: NavMeshData is null");
                return removeTile();
            }

            try
            {
                cachedNavMeshData = navMeshTilesCache.set(agentHalfExtents, changedTile, *recastMesh,
                                                          offMeshConnections, std::move(navMeshData));
            }
            catch (const InvalidArgument&)
            {
                cachedNavMeshData = navMeshTilesCache.get(agentHalfExtents, changedTile, *recastMesh,
                                                          offMeshConnections);
            }

            if (!cachedNavMeshData)
            {
                log("cache overflow");

                const auto locked = navMeshCacheItem.lock();
                auto& navMesh = locked->getValue();
                const auto tileRef = navMesh.getTileRefAt(x, y, 0);
                const auto removed = dtStatusSucceed(navMesh.removeTile(tileRef, nullptr, nullptr));
                const auto addStatus = navMesh.addTile(navMeshData.mValue.get(), navMeshData.mSize,
                                                       doNotTransferOwnership, 0, 0);

                if (dtStatusSucceed(addStatus))
                {
//.........这里部分代码省略.........

bool Sample_TileMesh::handleLoadSubTiles()
{
    if (!m_geom || !m_geom->getMesh())
    {
        m_ctx->log(RC_LOG_ERROR, "buildNavigation: Input mesh is not specified.");
        return false;
    }

    cleanup();

    const char* meshFilePath = m_geom->getMesh()->getFileName();
    char charBuff[4];
    memset(charBuff, 0, sizeof(charBuff));
    memcpy(charBuff, &meshFilePath[10], sizeof(char) * 3);
    int mapId = atoi(charBuff);
    // load and init dtNavMesh - read parameters from file
    int pathLen = strlen("Meshes/%03i.mmap") + 1;
    char *fileName = new char[pathLen];
    snprintf(fileName, pathLen, "Meshes/%03i.mmap", mapId);
    FILE* file = fopen(fileName, "rb");
    if (!file)
    {
        delete[] fileName;
        return false;
    }
    dtNavMeshParams params;
    int count = fread(&params, sizeof(dtNavMeshParams), 1, file);
    fclose(file);
    if (count != 1)
    {
        delete[] fileName;
        return false;
    }
    params.maxTiles = 25 * 25 * 9;

    dtNavMesh* mesh = dtAllocNavMesh();
    if (dtStatusFailed(mesh->init(&params)))
    {
        dtFreeNavMesh(mesh);
        delete[] fileName;
        return false;
    }
    delete[] fileName;

    memset(charBuff, 0, sizeof(charBuff));
    memcpy(charBuff, &meshFilePath[13], sizeof(char) * 2);
    int x = atoi(charBuff);
    memset(charBuff, 0, sizeof(charBuff));
    memcpy(charBuff, &meshFilePath[15], sizeof(char) * 2);
    int y = atoi(charBuff);

    for (int subRow = 0; subRow < 25; subRow++)
    {
        for (int subCol = 0; subCol < 25; subCol++)
        {
            // load this tile :: Meshes/MMMXXYY.mmtile
            pathLen = strlen("Meshes/%03i%02i%02i_____%02i%02i.mmtile") + 1;
            fileName = new char[pathLen];
            snprintf(fileName, pathLen, "Meshes/%03i%02i%02i_____%02i%02i.mmtile", mapId, x, y, subRow, subCol);

            file = fopen(fileName, "rb");
            if (!file)
            {
                delete[] fileName;
                continue;
            }
            delete[] fileName;

            // read header
            MmapTileHeader fileHeader;
            if (fread(&fileHeader, sizeof(MmapTileHeader), 1, file) != 1 || fileHeader.mmapMagic != MMAP_MAGIC)
            {
                fclose(file);
                continue;
            }

            unsigned char* data = (unsigned char*)dtAlloc(fileHeader.size, DT_ALLOC_PERM);

            size_t result = fread(data, fileHeader.size, 1, file);
            if (!result)
            {
                fclose(file);
                continue;
            }

            // Fix x/y
            dtMeshHeader* header = (dtMeshHeader*)data;
            header->x = header->x * 25 + subRow;
            header->y = header->y * 25 + subCol;

            fclose(file);

            dtTileRef tileRef = 0;

            // memory allocated for data is now managed by detour, and will be deallocated when the tile is removed
            if (!dtStatusSucceed(mesh->addTile(data, fileHeader.size, DT_TILE_FREE_DATA, 0, &tileRef)))
            {
                dtFree(data);
                continue;
            }
//.........这里部分代码省略.........

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

					// Handle end of path and off-mesh links when close enough.
					if (endOfPath && inRange(iterPos, steerPos, SLOP, 1.0f))
					{
						// Reached end of path.
						dtVcopy(iterPos, targetPos);
						if (m_nsmoothPath < MAX_SMOOTH)
						{
							dtVcopy(&m_smoothPath[m_nsmoothPath*3], iterPos);
							m_nsmoothPath++;
						}
						break;
					}
					else if (offMeshConnection && inRange(iterPos, steerPos, SLOP, 1.0f))
					{
						// Reached off-mesh connection.
						float startPos[3], endPos[3];
						
						// Advance the path up to and over the off-mesh connection.
						dtPolyRef prevRef = 0, polyRef = polys[0];
						int npos = 0;
						while (npos < npolys && polyRef != steerPosRef)
						{
							prevRef = polyRef;
							polyRef = polys[npos];
							npos++;
						}
						for (int i = npos; i < npolys; ++i)
							polys[i-npos] = polys[i];
						npolys -= npos;
						
						// Handle the connection.
						dtStatus status = m_navMesh->getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos);
						if (dtStatusSucceed(status))
						{
							if (m_nsmoothPath < MAX_SMOOTH)
							{
								dtVcopy(&m_smoothPath[m_nsmoothPath*3], startPos);
								m_nsmoothPath++;
								// Hack to make the dotted path not visible during off-mesh connection.
								if (m_nsmoothPath & 1)
								{
									dtVcopy(&m_smoothPath[m_nsmoothPath*3], startPos);
									m_nsmoothPath++;
								}
							}
							// Move position at the other side of the off-mesh link.
							dtVcopy(iterPos, endPos);
							float eh = 0.0f;
							m_navQuery->getPolyHeight(polys[0], iterPos, &eh);
							iterPos[1] = eh;
						}
					}
					
					// Store results.
					if (m_nsmoothPath < MAX_SMOOTH)
					{
						dtVcopy(&m_smoothPath[m_nsmoothPath*3], iterPos);
						m_nsmoothPath++;
					}
				}
			}

		}
		else
		{

void PathFinder::BuildPolyPath(const Vector3& startPos, const Vector3& 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