Ejemplo n.º 1
0
        public static void DrawPolygonGraph(PNavMesh pNavMesh)
        {
            Color c = Handles.color;

            foreach (PNavIsland island in pNavMesh.islands)
            {
                int index = 0;
                foreach (PNavPolygon polygon in island.graph.polygons)
                {
                    Handles.color = Color.green;
                    Handles.SphereHandleCap(0, (Vector3)polygon.Centroid3D, Quaternion.identity, 0.1f, EventType.Repaint);

                    Handles.Label((Vector3)polygon.Centroid3D + Vector3.forward * 0.5f, index.ToString());

                    foreach (PNavEdge edge in polygon.edges)
                    {
                        if (edge.hasOther)
                        {
                            PNavPolygon self  = island.graph.polygons[edge.selfPolygonIndex];
                            PNavPolygon other = island.graph.polygons[edge.otherPolygonIndex];
                            Handles.DrawLine((Vector3)self.Centroid3D, (Vector3)other.Centroid3D);
                        }
                    }

                    index++;
                }
            }
            Handles.color = c;
        }
Ejemplo n.º 2
0
        public PNavPolygon FindNearestPolygong(Fix64Vec2 point)
        {
            Fix64       min        = Fix64.FromDivision(1000, 1);
            PNavPolygon minPolygon = null;

            foreach (PNavPolygon polygon in graph.polygons)
            {
                Fix64 dis = Fix64Vec2.Distance(polygon.centroid, point);
                if (dis < min)
                {
                    min        = dis;
                    minPolygon = polygon;
                }
            }

            return(minPolygon);
        }
Ejemplo n.º 3
0
        public PNavEdge FindEdge(PNavPolygon currentPolygon, PNavPolygon nextPolygon, Fix64 width)
        {
            foreach (PNavEdge edge in nextPolygon.edges)
            {
                if (edge.hasOther && edge.otherPolygonIndex == currentPolygon.index)
                {
                    if (width > edge.width)
                    {
                        return(null);
                    }
                    else
                    {
                        return(edge);
                    }
                }
            }

            return(null);
        }
        public static void Process(PNavMesh pNavMesh)
        {
            using (new SProfiler($"Build polygon graph"))
            {
                foreach (PNavIsland island in pNavMesh.islands)
                {
                    Dictionary <long, PNavEdge> edgeMap = new Dictionary <long, PNavEdge>();
                    PNavPolygonGraph            graph   = new PNavPolygonGraph();

                    int indiceRead = 0;
                    for (int p = 0; p < island.polygonCount; p++)
                    {
                        int ic = island.indiceCountsOfPolygons[p];

                        Fix64Vec2[] verts   = new Fix64Vec2[ic];
                        int[]       indices = new int[ic];

                        for (int indiceOfPolygon = 0; indiceOfPolygon < ic; indiceOfPolygon++)
                        {
                            int      nodeIndex = island.indices[indiceRead];
                            PNavNode vertNode  = island.nodes[nodeIndex];
                            verts[indiceOfPolygon]   = new Fix64Vec2(vertNode.Center.x, vertNode.Center.z);
                            indices[indiceOfPolygon] = nodeIndex;
                            indiceRead++;
                        }

                        PNavPolygon polygon = new PNavPolygon(verts, ic);

                        //polygon index should match its index in graph.polygons list
                        polygon.index = p;
                        graph.AddPolygon(polygon);

                        BuildEdges(graph, indices, ic, polygon, edgeMap);
                    }

                    island.graph = graph;
                }
            }
        }
Ejemplo n.º 5
0
        public bool PositionOnThePath(Fix64Vec2 position, ref int index)
        {
            bool        positionIsOnThePath = false;
            int         checkedPolygon      = startIndex;
            PNavPolygon currentPolygon      = null;

            while ((MAX_CORNER_COUNT - checkedPolygon) > 0)
            {
                int         polygonIndex  = polygonIndexes[checkedPolygon];
                PNavPolygon polygon       = island.graph.polygons[polygonIndex];
                bool        insidePolygon = polygon.TestPoint(position);
                if (insidePolygon)
                {
                    currentPolygon      = polygon;
                    index               = checkedPolygon;
                    positionIsOnThePath = true;
                    break;
                }
                checkedPolygon++;
            }

            return(positionIsOnThePath);
        }
Ejemplo n.º 6
0
 public bool Equals(PNavPolygon other)
 {
     return(index == other.index);
 }
        static void BuildEdges(PNavPolygonGraph graph, int[] indices, int count, PNavPolygon polygon, Dictionary <long, PNavEdge> edgeMap)
        {
            int       previous        = 0;
            Fix64Vec2 previousVector2 = Fix64Vec2.zero;

            for (int i = 0; i < count; i++)
            {
                int       current        = indices[i];
                Fix64Vec2 currentVector2 = polygon.verts[i];

                if (i != 0)
                {
                    long key = KeyForIndices(previous, current);
                    if (edgeMap.ContainsKey(key))
                    {
                        PNavEdge edge = edgeMap[key];
                        edge.otherPolygonIndex = polygon.index;
                        edge.hasOther          = true;
                        PNavPolygon self = graph.polygons[edge.selfPolygonIndex];
                        edge.distance = Fix64Vec2.Distance(self.centroid, polygon.centroid);

                        PNavEdge newEdge = new PNavEdge(edge);
                        newEdge.SwapDirection();

                        polygon.edges.Add(newEdge);
                    }
                    else
                    {
                        PNavEdge edge = new PNavEdge();
                        edge.selfPolygonIndex = polygon.index;
                        edge.width            = Fix64Vec2.Distance(previousVector2, currentVector2);
                        edge.pointA           = previousVector2;
                        edge.pointB           = currentVector2;
                        edgeMap[key]          = edge;
                        polygon.edges.Add(edge);
                    }
                }

                previous        = current;
                previousVector2 = currentVector2;
            }

            int  lastCurrent = indices[0];
            long lastKey     = KeyForIndices(previous, lastCurrent);

            if (edgeMap.ContainsKey(lastKey))
            {
                PNavEdge edge = edgeMap[lastKey];
                edge.otherPolygonIndex = polygon.index;
                edge.hasOther          = true;
                PNavPolygon self = graph.polygons[edge.selfPolygonIndex];
                edge.distance = Fix64Vec2.Distance(self.centroid, polygon.centroid);

                PNavEdge newEdge = new PNavEdge(edge);
                newEdge.SwapDirection();

                polygon.edges.Add(newEdge);
            }
            else
            {
                PNavEdge edge = new PNavEdge();
                edge.selfPolygonIndex = polygon.index;
                edge.width            = Fix64Vec2.Distance(previousVector2, polygon.verts[0]);
                edge.pointA           = previousVector2;
                edge.pointB           = polygon.verts[0];
                edgeMap[lastKey]      = edge;
                polygon.edges.Add(edge);
            }
        }
Ejemplo n.º 8
0
        private void OnDrawGizmos()
        {
            if (!Application.isPlaying)
            {
                return;
            }

            if (_path == null)
            {
                return;
            }

            /*
             * if (_nextCorner != Fix64Vec3.zero)
             * {
             *  Gizmos.color = Color.red;
             *  Gizmos.DrawWireSphere((Vector3)_nextCorner, 0.1f);
             * }
             */
            Gizmos.color = Color.red;

            if (_path.Status == ParallelNavMeshPathStatus.Valid)
            {
                if (_path.startIndex != -1)
                {
                    PNavPolygon previousPolygon = null;
                    for (int index = _path.startIndex; index < 128; index++)
                    {
                        int polygonIndex = _path.polygonIndexes[index];

                        PNavPolygon currentPolygon = _path.island.graph.polygons[polygonIndex];

                        if (previousPolygon != null)
                        {
                            Gizmos.DrawLine((Vector3)previousPolygon.Centroid3D, (Vector3)currentPolygon.Centroid3D);
                        }

                        previousPolygon = currentPolygon;
                    }

                    if (_minLeftVector != Fix64Vec2.zero)
                    {
                        Fix64Vec2 pos   = _waypoints[_waypoints.Count - 1];
                        Vector3   pos3D = new Vector3((float)pos.x, 0, (float)pos.y);

                        Gizmos.color = Color.blue;
                        Vector3 left3D = new Vector3((float)_minLeft.x, 0, (float)_minLeft.y);
                        Gizmos.DrawLine(pos3D, left3D);
                        Gizmos.DrawWireSphere(left3D, 0.1f);

                        Gizmos.color = Color.magenta;
                        Vector3 right3D = new Vector3((float)_minRight.x, 0, (float)_minRight.y);

                        Gizmos.DrawLine(pos3D, right3D);
                        Gizmos.DrawWireSphere(right3D, 0.1f);
                    }
                }

                Gizmos.color = Color.white;
                for (int i = 0; i < _waypoints.Count; i++)
                {
                    Fix64Vec2 wp   = _waypoints[i];
                    Vector3   wp3D = new Vector3((float)wp.x, 0, (float)wp.y);
                    Gizmos.DrawWireSphere(wp3D, 0.1f);

                    if (i == 0)
                    {
                        continue;
                    }
                    else
                    {
                        Fix64Vec2 prev     = _waypoints[i - 1];
                        Vector3   prevWp3D = new Vector3((float)prev.x, 0, (float)prev.y);
                        Gizmos.DrawLine(prevWp3D, wp3D);
                    }
                }
            }
        }
Ejemplo n.º 9
0
        //http://digestingduck.blogspot.com/2010/03/simple-stupid-funnel-algorithm.html
        void ProcessNextPolygon()
        {
            //Debug.Log($"ProcessNextPolygon {_pathPolygonIndex}");
            Fix64Vec2 pos = _waypoints[_waypoints.Count - 1];

            if (_pathPolygonIndex == 128)
            {
                //Debug.Log("LAST POLYGON");

                if (_minLeftVector == Fix64Vec2.zero)
                {
                    //Debug.Log("GO to destination directly");
                    _waypoints.Add(_path.Destination2D);

                    ResetPathValues();
                    return;
                }

                Fix64Vec2 destinationVector = _path.Destination2D - pos;

                bool destinationLeftOfMinLeft   = Fix64Vec2.Cross(_minLeftVector, destinationVector) > Fix64.zero;
                bool destinationRightOfMinRight = Fix64Vec2.Cross(_minRightVector, destinationVector) < Fix64.zero;

                bool lastRecalculate = false;
                bool useLeft         = true;

                if (destinationLeftOfMinLeft && destinationRightOfMinRight)
                {
                    //when both are too be added
                    //use the one that is closer to pos
                    Fix64 leftDis  = Fix64Vec2.Distance(_minLeft, pos);
                    Fix64 rightDis = Fix64Vec2.Distance(_minRight, pos);

                    if (leftDis > rightDis)
                    {
                        useLeft = false;
                    }
                }

                if (destinationLeftOfMinLeft && useLeft)
                {
                    _waypoints.Add(_minLeft);
                    pos = _minLeft;
                    _pathPolygonIndex = _leftPolygonIndex;
                    _previousPolygon  = _leftPreviousPolygon;
                    lastRecalculate   = true;
                }

                if (destinationRightOfMinRight && !lastRecalculate)
                {
                    _waypoints.Add(_minRight);
                    pos = _minRight;
                    _pathPolygonIndex = _rightPolygonIndex;
                    _previousPolygon  = _rightPreviousPolygon;
                    lastRecalculate   = true;
                }

                if (!lastRecalculate)
                {
                    //Debug.Log("GO to destination directly");
                    _waypoints.Add(_path.Destination2D);

                    ResetPathValues();
                }
                else
                {
                    _minLeftVector = Fix64Vec2.zero;
                }

                return;
            }

            if (_finishedProcessing)
            {
                return;
            }

            int polygonIndex = _path.polygonIndexes[_pathPolygonIndex];

            PNavPolygon currentPolygon = _path.island.graph.polygons[polygonIndex];

            PNavEdge edge = _path.FindEdge(_previousPolygon, currentPolygon, Fix64.zero);

            if (edge == null)
            {
                Debug.LogError("No Edge found");
                return;
            }

            _previousPolygon = currentPolygon;
            _pathPolygonIndex++;

            Fix64Vec2 ab = edge.pointB - edge.pointA;
            Fix64Vec2 ba = edge.pointA - edge.pointB;

            Fix64Vec2 pa    = edge.pointA + ab.normalized * error;
            Fix64Vec2 pb    = edge.pointB + ba.normalized * error;
            Fix64Vec2 vA    = pa - pos;
            Fix64Vec2 vB    = pb - pos;
            Fix64     c     = Fix64Vec2.Cross(vA, vB);
            Fix64Vec2 left  = pb;
            Fix64Vec2 right = pa;

            if (c > Fix64.zero)
            {
                //Debug.Log($"POSITVE: pointA is on the right. pA={pa} pB={pb} pos={pos}");
            }
            else
            {
                //Debug.Log($"NEGTIVE: pointA is on the left. pA={pa} pB={pb} pos={pos}");
                right = pb;
                left  = pa;
            }

            if (_minLeftVector == Fix64Vec2.zero)
            {
                _minLeft              = left;
                _minRight             = right;
                _minLeftVector        = left - pos;
                _minRightVector       = right - pos;
                _leftPolygonIndex     = _pathPolygonIndex;
                _rightPolygonIndex    = _pathPolygonIndex;
                _leftPreviousPolygon  = _previousPolygon;
                _rightPreviousPolygon = _previousPolygon;
                return;
            }

            Fix64Vec2 newLeftVector  = left - pos;
            Fix64Vec2 newRightVector = right - pos;

            bool newLeftIsRightToOldRight = Fix64Vec2.Cross(_minRightVector, newLeftVector) < Fix64.zero;
            bool newRightIsLeftToOldLeft  = Fix64Vec2.Cross(_minLeftVector, newRightVector) > Fix64.zero;

            bool recalculateNewVectors = false;

            if (newLeftIsRightToOldRight)
            {
                //move to old right and recalculate
                _waypoints.Add(_minRight);
                pos = _minRight;
                _pathPolygonIndex     = _rightPolygonIndex;
                _previousPolygon      = _rightPreviousPolygon;
                recalculateNewVectors = true;
            }

            if (newRightIsLeftToOldLeft && !recalculateNewVectors)
            {
                //move to old left and recalculate
                _waypoints.Add(_minLeft);
                pos = _minLeft;
                _pathPolygonIndex     = _leftPolygonIndex;
                _previousPolygon      = _leftPreviousPolygon;
                recalculateNewVectors = true;
            }

            if (recalculateNewVectors)
            {
                _minLeftVector = Fix64Vec2.zero;
                return;
            }
            else
            {
                bool newLeftIsOk  = Fix64Vec2.Cross(_minLeftVector, newLeftVector) <= Fix64.zero;
                bool newRightIsOk = Fix64Vec2.Cross(_minRightVector, newRightVector) >= Fix64.zero;

                if (newLeftIsOk)
                {
                    _minLeft             = left;
                    _minLeftVector       = newLeftVector;
                    _leftPolygonIndex    = _pathPolygonIndex;
                    _leftPreviousPolygon = _previousPolygon;
                }

                if (newRightIsOk)
                {
                    _minRight             = right;
                    _minRightVector       = newRightVector;
                    _rightPolygonIndex    = _pathPolygonIndex;
                    _rightPreviousPolygon = _previousPolygon;
                }
            }
        }
Ejemplo n.º 10
0
        public bool NextCorner1(Fix64Vec3 position, Fix64 width, ref Fix64Vec3 corner, ref int index)
        {
            //if corner not found stay at current position
            corner = position;

            if (Status != ParallelNavMeshPathStatus.Valid)
            {
                return(false);
            }

            if (index < 0)
            {
                index = startIndex;
            }

            if (index == MAX_CORNER_COUNT)
            {
                //reached last polygon, invalidate path
                index  = -1;
                Status = ParallelNavMeshPathStatus.Invalid;
                return(false);
            }

            Fix64Vec2 testPosition = new Fix64Vec2(position.x, position.z);

            int         polygonIndex   = polygonIndexes[index];
            PNavPolygon currentPolygon = island.graph.polygons[polygonIndex];

            int nextIndex = index + 1;

            if (nextIndex >= MAX_CORNER_COUNT)
            {
                //reached final polygon
                index  = MAX_CORNER_COUNT;
                corner = Destination;
                return(false);
            }

            int         nextPolygonIndex = polygonIndexes[nextIndex];
            PNavPolygon nextPolygon      = island.graph.polygons[nextPolygonIndex];
            PNavEdge    edge             = FindEdge(currentPolygon, nextPolygon, width);

            if (edge != null)
            {
                Fix64Vec2 pointOnEdge = Fix64Math.FindNearestPointOnLine(testPosition, edge.pointA, edge.pointB);
                Fix64     distanceToA = Fix64Vec2.Distance(pointOnEdge, edge.pointA);
                Fix64     distanceToB = edge.width - distanceToA;
                Fix64Vec2 ba          = edge.pointB - edge.pointA;
                ba = ba.normalized;
                Fix64 halfWidth = width / Fix64.two;
                if (distanceToA < halfWidth)
                {
                    pointOnEdge = edge.pointA + ba * halfWidth;
                }
                else if (distanceToB < halfWidth)
                {
                    pointOnEdge = edge.pointB - ba * halfWidth;
                }

                corner = new Fix64Vec3(pointOnEdge.x, position.y, pointOnEdge.y);
                index  = nextIndex;
                return(true);
            }

            return(false);
        }
Ejemplo n.º 11
0
        public bool NextCorner(Fix64Vec3 position, Fix64 width, ref Fix64Vec3 corner, ref int index)
        {
            //if corner not found stay at current position
            corner = position;

            if (Status != ParallelNavMeshPathStatus.Valid)
            {
                return(false);
            }

            index = BoundaryCheckIndex(index);

            if (index == -1)
            {
                Status = ParallelNavMeshPathStatus.Invalid;
                return(false);
            }

            Fix64Vec2 testPosition = new Fix64Vec2(position.x, position.z);

            Fix64Vec2 pa         = Fix64Vec2.zero;
            Fix64Vec2 pb         = Fix64Vec2.zero;
            bool      pointABSet = false;

            if (polygonIndexes == null)
            {
                //destination polygon and start polygon is the same
                index = MAX_CORNER_COUNT;
            }

            while (index <= MAX_CORNER_COUNT)
            {
                if (index == MAX_CORNER_COUNT)
                {
                    //reached final polygon
                    if (pointABSet)
                    {
                        //check if destination is in the span of pa and pb
                        Fix64Vec2 des       = new Fix64Vec2(Destination.x, Destination.z);
                        Fix64Vec2 vd        = des - testPosition;
                        Fix64Vec2 va        = pa - testPosition;
                        Fix64Vec2 vb        = pb - testPosition;
                        bool      desInSpan = Fix64Math.InSpan(vd, va, vb);
                        if (!desInSpan)
                        {
                            Fix64Vec2 pointOnEdge = FindNearestPointOnEdge(testPosition, pa, pb, width);
                            index--;
                            corner = new Fix64Vec3(pointOnEdge.x, position.y, pointOnEdge.y);
                            return(true);
                        }
                    }

                    corner = Destination;
                    break;
                }

                int         nextIndex        = index + 1;
                int         polygonIndex     = polygonIndexes[index];
                PNavPolygon currentPolygon   = island.graph.polygons[polygonIndex];
                int         nextPolygonIndex = polygonIndexes[nextIndex];
                PNavPolygon nextPolygon      = island.graph.polygons[nextPolygonIndex];

                PNavEdge edge = FindEdge(currentPolygon, nextPolygon, width);

                if (edge != null)
                {
                    if (!pointABSet)
                    {
                        pa         = edge.pointA;
                        pb         = edge.pointB;
                        pointABSet = true;
                    }
                    else
                    {
                        Fix64Vec2 va  = pa - testPosition;
                        Fix64Vec2 vb  = pb - testPosition;
                        Fix64     AXB = Fix64Vec2.Cross(va, vb);
                        Fix64     BXA = Fix64Vec2.Cross(vb, va);

                        Fix64Vec2 pa1 = edge.pointA;
                        Fix64Vec2 pb1 = edge.pointB;

                        Fix64Vec2 va1   = pa1 - testPosition;
                        Fix64Vec2 vb1   = pb1 - testPosition;
                        Fix64     A1XB1 = Fix64Vec2.Cross(va1, vb1);

                        bool sameDirection = A1XB1 * AXB > Fix64.zero;

                        bool va1InsideOfSpan = Fix64Math.InSpan(va1, va, vb);
                        bool vb1InsideOfSpan = Fix64Math.InSpan(vb1, va, vb);

                        if (va1InsideOfSpan)
                        {
                            if (sameDirection)
                            {
                                bool valid = IsValidEdgePoint(testPosition, pb, pa1, width);
                                if (valid)
                                {
                                    pa = pa1;
                                }
                                else
                                {
                                    va1InsideOfSpan = false;
                                }
                            }
                            else
                            {
                                bool valid = IsValidEdgePoint(testPosition, pa, pa1, width);
                                if (valid)
                                {
                                    pb = pa1;
                                }
                                else
                                {
                                    va1InsideOfSpan = false;
                                }
                            }
                        }


                        if (vb1InsideOfSpan)
                        {
                            if (sameDirection)
                            {
                                bool valid = IsValidEdgePoint(testPosition, pa, pb1, width);
                                if (valid)
                                {
                                    pb = pb1;
                                }
                                else
                                {
                                    vb1InsideOfSpan = false;
                                }
                            }
                            else
                            {
                                bool valid = IsValidEdgePoint(testPosition, pb, pb1, width);
                                if (valid)
                                {
                                    pa = pb1;
                                }
                                else
                                {
                                    vb1InsideOfSpan = false;
                                }
                            }
                        }

                        if (!va1InsideOfSpan && !vb1InsideOfSpan)
                        {
                            Fix64Vec2 pointOnEdge = FindNearestPointOnEdge(testPosition, pa, pb, width);

                            corner = new Fix64Vec3(pointOnEdge.x, position.y, pointOnEdge.y);
                            index--;
                            return(true);
                        }
                    }
                }
                else
                {
                    break;
                }

                index = BoundaryCheckIndex(nextIndex);
            }

            return(false);
        }
Ejemplo n.º 12
0
 public void AddPolygon(PNavPolygon polygon)
 {
     polygons.Add(polygon);
 }
Ejemplo n.º 13
0
        public PNavMeshPath CalculatePath(Fix64Vec3 start, Fix64Vec3 end)
        {
            Fix64Vec2   startPosition = new Fix64Vec2(start.x, start.z);
            Fix64Vec2   endPosition   = new Fix64Vec2(end.x, end.z);
            PNavPolygon startPolygon  = null;
            PNavPolygon endPolygon    = null;

            bool          sameIsland = false;
            NavMeshAStart astart     = null;

            PNavMeshPath result = new PNavMeshPath();

            result.Destination   = end;
            result.Destination2D = endPosition;
            result.Status        = ParallelNavMeshPathStatus.Invalid;
            result.navMesh       = navMesh;

            PNavIsland endIsland   = null;
            PNavIsland startIsland = null;

            foreach (PNavIsland island in navMesh.islands)
            {
                bool foundStart = false;
                bool foundEnd   = false;

                foreach (PNavPolygon polygon in island.graph.polygons)
                {
                    bool isStart = polygon.TestPoint(startPosition);
                    bool isEnd   = polygon.TestPoint(endPosition);

                    if (isStart && isEnd)
                    {
                        result.Status     = ParallelNavMeshPathStatus.Valid;
                        result.startIndex = -1;
                        return(result);
                    }

                    if (isStart)
                    {
                        startPolygon = polygon;
                        startIsland  = island;
                        foundStart   = true;
                    }

                    if (isEnd)
                    {
                        endPolygon = polygon;
                        endIsland  = island;
                        foundEnd   = true;
                    }
                }

                if (foundStart && foundEnd)
                {
                    sameIsland    = true;
                    astart        = astartDictionary[island];
                    result.island = island;
                    break;
                }

                if (sameIsland)
                {
                    break;
                }
            }

            if (startPolygon != null && endPolygon == null)
            {
                //we find the polygon that is closest to the end position in the start position island
                sameIsland    = true;
                astart        = astartDictionary[startIsland];
                result.island = startIsland;
                endPolygon    = startIsland.FindNearestPolygong(endPosition);
            }
            else if (startPolygon == null && endPolygon != null)
            {
                //we find the polygon that is closest to the start position in the end position island
                sameIsland    = true;
                astart        = astartDictionary[endIsland];
                result.island = endIsland;
                startPolygon  = endIsland.FindNearestPolygong(startPosition);
            }
            else if (startPolygon == null && endPolygon == null)
            {
                //we find the polygon that is closest to the start position
                //we then find the polygon that is closest to the end position in the same island
                Fix64       minStart        = Fix64.FromDivision(1000, 1);
                PNavPolygon minStartPolygon = null;
                PNavIsland  minStartIsland  = null;

                foreach (PNavIsland island in navMesh.islands)
                {
                    foreach (PNavPolygon polygon in island.graph.polygons)
                    {
                        Fix64 dis = Fix64Vec2.Distance(polygon.centroid, startPosition);
                        if (dis < minStart)
                        {
                            minStart        = dis;
                            minStartPolygon = polygon;
                            minStartIsland  = island;
                        }
                    }
                }


                sameIsland    = true;
                astart        = astartDictionary[minStartIsland];
                result.island = minStartIsland;
                startPolygon  = minStartPolygon;

                endPolygon = minStartIsland.FindNearestPolygong(endPosition);
            }

            if (sameIsland)
            {
                if (astart != null && startPolygon != null && endPolygon != null)
                {
                    NavMeshAStarNode startNode = astart.FindNode(startPolygon.index);
                    NavMeshAStarNode endNode   = astart.FindNode(endPolygon.index);
                    NavMeshAStarNode lastNode  = null;

                    using (new SProfiler("Pathfinding"))
                    {
                        astart.PrePathFinding();

                        startNode.startPoint = startPosition;
                        endNode.isLastNode   = true;
                        endNode.endPoint     = endPosition;

                        lastNode = astart.FindPath(startNode, endNode);
                    }

                    result.polygonIndexes = new int[PNavMeshPath.MAX_CORNER_COUNT];

                    int startIndex = 127;

                    while (lastNode != null)
                    {
                        result.polygonIndexes[startIndex] = lastNode.UserObject.index;
                        lastNode = (NavMeshAStarNode)lastNode.Parent;
                        startIndex--;
                    }

                    result.startIndex = startIndex + 1;
                    result.Status     = ParallelNavMeshPathStatus.Valid;
                }
            }

            return(result);
        }