コード例 #1
0
 public void Dispose()
 {
     _F_.Dispose();
     solution.Dispose();
     frontier.Dispose();
     visited.Dispose();
     neighbours.Dispose();
 }
コード例 #2
0
        public void Execute()
        {
            int  pathNodeArraySize = clusterSize * clusterSize;
            int2 startPosition     = nodePositions[startNode];
            int2 endPosition       = nodePositions[endNode];

            // Node sets
            NativeMinHeap openSet = new NativeMinHeap();

            openSet.Initialize(pathNodeArraySize, 0, pathNodeArraySize + 1);

            NativeArray <int>  localIndexArray = new NativeArray <int>(pathNodeArraySize, Allocator.Temp);
            NativeArray <int>  graphIndexArray = new NativeArray <int>(pathNodeArraySize, Allocator.Temp);
            NativeArray <int>  parentArray     = new NativeArray <int>(pathNodeArraySize, Allocator.Temp);
            NativeArray <int>  hCostArray      = new NativeArray <int>(pathNodeArraySize, Allocator.Temp);
            NativeArray <int>  gCostArray      = new NativeArray <int>(pathNodeArraySize, Allocator.Temp);
            NativeArray <int>  fCostArray      = new NativeArray <int>(pathNodeArraySize, Allocator.Temp);
            NativeArray <bool> openArray       = new NativeArray <bool>(pathNodeArraySize, Allocator.Temp);
            NativeArray <bool> closedArray     = new NativeArray <bool>(pathNodeArraySize, Allocator.Temp);

            // Initialize nodes from ReadOnly grid array
            for (int localRow = 0; localRow < clusterSize; localRow++)
            {
                for (int localCol = 0; localCol < clusterSize; localCol++)
                {
                    int localIndex      = localCol + localRow * clusterSize;
                    int graphCol        = localCol + clusterSize * clusterPosition.x;
                    int graphRow        = localRow + clusterSize * clusterPosition.y;
                    int graphArrayIndex = graphCol + graphRow * numCells;

                    localIndexArray[localIndex] = localIndex;
                    graphIndexArray[localIndex] = graphArrayIndex;
                    parentArray[localIndex]     = -1;
                    gCostArray[localIndex]      = int.MaxValue;
                    openArray[localIndex]       = false;
                    closedArray[localIndex]     = false;
                }
            }

            // Get and cache the start and end pathNodeIndices
            int endNodeX   = endPosition.x - clusterPosition.x * clusterSize;
            int endNodeY   = endPosition.y - clusterPosition.y * clusterSize;
            int startNodeX = startPosition.x - clusterPosition.x * clusterSize;
            int startNodeY = startPosition.y - clusterPosition.y * clusterSize;

            int endNodeIndex   = endNodeX + endNodeY * clusterSize;
            int startNodeIndex = startNodeX + startNodeY * clusterSize;

            // Initialize the starting pathNode
            int  graphIndex   = graphIndexArray[startNodeIndex];
            int2 nodePosition = nodePositions[graphIndex];

            int hCost = ManhattenDistance(nodePosition, endPosition);

            gCostArray[startNodeIndex] = 0;
            hCostArray[startNodeIndex] = hCost;
            fCostArray[startNodeIndex] = hCost;
            openArray[startNodeIndex]  = true;

            // Add the start pathNode to the openSet
            openSet.Enqueue(startNodeIndex, hCost);

            while (openSet.Length > 0)
            {
                // Cache the pathNodeIndex we are working with during this iteration
                int currentNodeIndex = openSet.DequeueMin(closedArray);

                // Break if we reached our goal
                if (currentNodeIndex == endNodeIndex)
                {
                    break;
                }

                openArray[currentNodeIndex]   = false;
                closedArray[currentNodeIndex] = true;

                int2 currentNodePosition = nodePositions[graphIndexArray[currentNodeIndex]];
                int2 neighbourKey        = neighbourKeys[graphIndexArray[currentNodeIndex]];

                for (int i = neighbourKey.x; i < neighbourKey.y; i++)
                {
                    int2 neighbourPosition      = nodePositions[neighbours[i]];
                    int2 localNeighbourPosition = neighbourPosition - clusterPosition * clusterSize;
                    int  localNeighbourIndex    = localNeighbourPosition.x + localNeighbourPosition.y * clusterSize;

                    // Skip if its closed (already searched)
                    if (closedArray[localNeighbourIndex])
                    {
                        continue;
                    }

                    // Calculate the cost to move from current node to neighbour node
                    int distanceCost  = ManhattenDistance(currentNodePosition, neighbourPosition);
                    int tentativeCost = gCostArray[currentNodeIndex] + distanceCost;

                    if (tentativeCost < gCostArray[localNeighbourIndex])
                    {
                        int newHCost = ManhattenDistance(neighbourPosition, endPosition);

                        parentArray[localNeighbourIndex] = currentNodeIndex;
                        hCostArray[localNeighbourIndex]  = newHCost;
                        gCostArray[localNeighbourIndex]  = tentativeCost;
                        fCostArray[localNeighbourIndex]  = tentativeCost + hCost;

                        if (!openArray[localNeighbourIndex])
                        {
                            openArray[localNeighbourIndex] = true;
                            openSet.Enqueue(localNeighbourIndex, fCostArray[localNeighbourIndex]);
                        }
                    }
                }
            }

            NativeList <int2> pathFound    = new NativeList <int2>(Allocator.Temp);
            NativeList <int2> smoothedPath = new NativeList <int2>(Allocator.Temp);
            int  nodeIndex  = endNodeIndex;
            bool smoothPath = true;

            if (parentArray[nodeIndex] == -1)
            {
                openSet.Dispose();
                localIndexArray.Dispose();
                graphIndexArray.Dispose();
                parentArray.Dispose();
                hCostArray.Dispose();
                gCostArray.Dispose();
                fCostArray.Dispose();
                openArray.Dispose();
                closedArray.Dispose();
                pathFound.Dispose();
                smoothedPath.Dispose();

                path = new NativeList <int2>(Allocator.Temp);
            }
            else
            {
                pathFound.Add(endPosition);
                smoothedPath.Add(endPosition);

                while (parentArray[nodeIndex] != -1)
                {
                    pathFound.Add(nodePositions[graphIndexArray[parentArray[nodeIndex]]]);
                    //smoothedPath.Add( nodePositions[ graphIndexArray[ parentArray[ nodeIndex ] ] ] );
                    nodeIndex = parentArray[nodeIndex];
                }
            }

            if (smoothPath && pathFound.Length > 2)   // If its less than or equal 2 theres no need to smooth the path
            {
                int  fromIndex = 0;
                bool foundPath = false;

                while (!foundPath)
                {
                    int currentIndex = fromIndex + 2; // Because the next index is always going to be in line of sight

                    if (currentIndex > pathFound.Length - 1)
                    {
                        break;
                    }

                    while (true)
                    {
                        int stopIndex           = currentIndex - 1;
                        int graphNodeArrayIndex = pathFound[stopIndex].x + pathFound[stopIndex].y * numCells;

                        int2 start = pathFound[fromIndex];
                        int2 end   = pathFound[currentIndex];

                        if (!LOS(start.x, start.y, end.x, end.y))
                        {
                            smoothedPath.Add(nodePositions[graphNodeArrayIndex]);
                            fromIndex = stopIndex;
                            break;
                        }
                        else
                        {
                            if (currentIndex >= pathFound.Length - 1)
                            {
                                foundPath = true;
                                break;
                            }
                            currentIndex++;
                        }
                    }
                }
            }

            openSet.Dispose();
            localIndexArray.Dispose();
            graphIndexArray.Dispose();
            parentArray.Dispose();
            hCostArray.Dispose();
            gCostArray.Dispose();
            fCostArray.Dispose();
            openArray.Dispose();
            closedArray.Dispose();
            pathFound.Dispose();

            for (int i = 0; i < smoothedPath.Length; i++)
            {
                path.Add(smoothedPath[i]);
            }

            smoothedPath.Dispose();
        }
        private NativeList <int3> FindAbstractPath(NativeList <int> startNodes, int2 startPosition, int2 endPosition)
        {
            #region DATA SETUP

            int EDGE_ARRAY_SIZE = graphIntraEdges.Length;

            // Node sets
            NativeMinHeap openSet = new NativeMinHeap();
            openSet.Initialize(EDGE_ARRAY_SIZE, -1, EDGE_ARRAY_SIZE + 10);
            NativeArray <int>            graphNodeIntraIndexArray = new NativeArray <int>(EDGE_ARRAY_SIZE, Allocator.Temp);
            NativeArray <int>            graphNodeInterIndexArray = new NativeArray <int>(EDGE_ARRAY_SIZE, Allocator.Temp);
            NativeArray <int>            parentEdgeArray          = new NativeArray <int>(EDGE_ARRAY_SIZE, Allocator.Temp);
            NativeArray <int>            parentPathIndexArray     = new NativeArray <int>(EDGE_ARRAY_SIZE, Allocator.Temp);
            NativeArray <int>            hCostArray  = new NativeArray <int>(EDGE_ARRAY_SIZE, Allocator.Temp);
            NativeArray <int>            gCostArray  = new NativeArray <int>(EDGE_ARRAY_SIZE, Allocator.Temp);
            NativeArray <int>            fCostArray  = new NativeArray <int>(EDGE_ARRAY_SIZE, Allocator.Temp);
            NativeArray <Blittable_Bool> openArray   = new NativeArray <Blittable_Bool>(EDGE_ARRAY_SIZE, Allocator.Temp);
            NativeArray <Blittable_Bool> closedArray = new NativeArray <Blittable_Bool>(EDGE_ARRAY_SIZE, Allocator.Temp);

            void DisposeNativeContainers()
            {
                openSet.Dispose();
                graphNodeIntraIndexArray.Dispose();
                graphNodeInterIndexArray.Dispose();
                parentEdgeArray.Dispose();
                parentPathIndexArray.Dispose();
                hCostArray.Dispose();
                gCostArray.Dispose();
                fCostArray.Dispose();
                openArray.Dispose();
                closedArray.Dispose();
            }

            // Initialize default array values
            for (int i = 0; i < EDGE_ARRAY_SIZE; i++)
            {
                graphNodeIntraIndexArray[i] = graphIntraEdges[i];
                graphNodeInterIndexArray[i] = graphInterEdges[i];
                parentEdgeArray[i]          = -1;
                parentPathIndexArray[i]     = -1;
                gCostArray[i]  = int.MaxValue;
                openArray[i]   = false;
                closedArray[i] = false;
            }

            // Initialize the starting nodes(edges) from the parameter
            for (int i = 0; i < startNodes.Length; i++)
            {
                int nodeIndex = startNodes[i];
                int edgeIndex = graphNodeToEdge[nodeIndex];

                hCostArray[edgeIndex] = ManhattenDistance(graphNodePositions[nodeIndex], endPosition);
                gCostArray[edgeIndex] = ManhattenDistance(startPosition, graphNodePositions[nodeIndex]);
                fCostArray[edgeIndex] = gCostArray[edgeIndex] + hCostArray[edgeIndex];

                openSet.Enqueue(edgeIndex, fCostArray[edgeIndex]);
            }

            // So we can test when we have reached the end
            int2 endClusterPosition = new int2(
                endPosition.x / graphClusterLength,
                endPosition.y / graphClusterLength);

            int endEdgeIndex = -1;

            #endregion
            #region FIND PATH

            while (openSet.Length > 0)
            {
                int currentEdge      = openSet.DequeueMin(closedArray);
                int currentInterNode = graphNodeInterIndexArray[currentEdge];

                int2 clusterPosition = new int2(
                    graphNodePositions[currentInterNode].x / graphClusterLength,
                    graphNodePositions[currentInterNode].y / graphClusterLength);

                if (clusterPosition.Equals(endClusterPosition))
                {
                    endEdgeIndex = currentEdge;
                    break;
                }

                openArray[currentEdge]   = false;
                closedArray[currentEdge] = true;

                int2 interNodeNeighbours =
                    graphEdgeNeighbourList[currentInterNode];
                for (int i = interNodeNeighbours.x; i < interNodeNeighbours.y; i++)
                {
                    int neighbourNode = graphEdgeNeighbours[i];
                    int neighbourEdge = graphNodeToEdge[neighbourNode];

                    if (closedArray[neighbourEdge])
                    {
                        continue;
                    }

                    int distanceCost = ManhattenDistance(
                        graphNodePositions[currentInterNode], graphNodePositions[neighbourNode]);
                    int tentativeCost =
                        distanceCost + gCostArray[currentEdge];

                    if (tentativeCost < gCostArray[neighbourEdge])
                    {
                        int newHCost = ManhattenDistance(graphNodePositions[neighbourNode], endPosition);
                        parentEdgeArray[neighbourEdge]      = currentEdge;
                        parentPathIndexArray[neighbourEdge] = i;
                        hCostArray[neighbourEdge]           = newHCost;
                        gCostArray[neighbourEdge]           = tentativeCost;
                        fCostArray[neighbourEdge]           = newHCost + tentativeCost;

                        if (!openArray[neighbourEdge])
                        {
                            openArray[neighbourEdge] = true;
                            openSet.Enqueue(neighbourEdge, fCostArray[neighbourEdge]);
                        }
                    }
                }
            }

            #endregion
            #region TRACE PATH

            NativeList <int3> pathPositions = new NativeList <int3>(Allocator.Temp);

            if (endEdgeIndex != -1)
            {
                // Add the end position and the inter-node it connects to
                int interNodeIndex =
                    graphNodeInterIndexArray[endEdgeIndex];
                pathPositions.Add(new int3(
                                      endPosition.x,
                                      endPosition.y,
                                      0));
                pathPositions.Add(new int3(
                                      graphNodePositions[interNodeIndex].x,
                                      graphNodePositions[interNodeIndex].y,
                                      0));

                // Trace the path
                int edgeIndex = endEdgeIndex;
                while (parentEdgeArray[edgeIndex] != -1)
                {
                    int3 pathNodeData = new int3(
                        graphNodePositions[graphNodeIntraIndexArray[edgeIndex]].x,
                        graphNodePositions[graphNodeIntraIndexArray[edgeIndex]].y,
                        parentPathIndexArray[edgeIndex]);
                    pathPositions.Add(pathNodeData);
                    edgeIndex = parentEdgeArray[edgeIndex];

                    /*int2 pathPositionKey =
                     *  graphEdgePaths[ parentPathIndexArray[ edgeIndex ] ];
                     * for ( int i = pathPositionKey.x; i < pathPositionKey.y; i++ )
                     *  pathPositions.Add( graphEdgePathPositions[ i ] );*/
                }

                // Add the internode of the first edge to the path and the starting position
                int interEdge =
                    graphNodeToEdge[graphNodeInterIndexArray[edgeIndex]];
                int interNode =
                    graphNodeInterIndexArray[interEdge];

                pathPositions.Add(new int3(
                                      graphNodePositions[interNode].x,
                                      graphNodePositions[interNode].y,
                                      0));
                pathPositions.Add(new int3(
                                      startPosition.x,
                                      startPosition.y,
                                      0));

                DisposeNativeContainers();
                return(pathPositions);
            }
            else
            {
                DisposeNativeContainers();
                return(pathPositions);
            }

            #endregion
        }
        private NativeList <int2> FindLowLevelPathInCluster(int2 startPosition, int2 endPosition, int2 clusterPosition)
        {
            #region DATA SETUP

            int PATH_NODE_ARRAY_SIZE = graphClusterLength * graphClusterLength;

            // Node sets
            NativeMinHeap openSet = new NativeMinHeap();
            openSet.Initialize(PATH_NODE_ARRAY_SIZE, -1, PATH_NODE_ARRAY_SIZE + 1);
            NativeArray <int>            localIndexArray = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp);
            NativeArray <int>            graphIndexArray = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp);
            NativeArray <int>            parentArray     = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp);
            NativeArray <int>            hCostArray      = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp);
            NativeArray <int>            gCostArray      = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp);
            NativeArray <int>            fCostArray      = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp);
            NativeArray <Blittable_Bool> openArray       = new NativeArray <Blittable_Bool>(PATH_NODE_ARRAY_SIZE, Allocator.Temp);
            NativeArray <Blittable_Bool> closedArray     = new NativeArray <Blittable_Bool>(PATH_NODE_ARRAY_SIZE, Allocator.Temp);

            // Initialize nodes from ReadOnly grid array
            for (int localRow = 0; localRow < graphClusterLength; localRow++)
            {
                for (int localCol = 0; localCol < graphClusterLength; localCol++)
                {
                    int localIndex      = localCol + localRow * graphClusterLength;
                    int graphCol        = localCol + graphClusterLength * clusterPosition.x;
                    int graphRow        = localRow + graphClusterLength * clusterPosition.y;
                    int graphArrayIndex = graphCol + graphRow * graphCellLength;

                    localIndexArray[localIndex] = localIndex;
                    graphIndexArray[localIndex] = graphArrayIndex;
                    parentArray[localIndex]     = -1;
                    gCostArray[localIndex]      = int.MaxValue;
                    openArray[localIndex]       = false;
                    closedArray[localIndex]     = false;
                }
            }

            // Get and cache the start and end pathNodeIndices
            int endNodeX       = endPosition.x - clusterPosition.x * graphClusterLength;
            int endNodeY       = endPosition.y - clusterPosition.y * graphClusterLength;
            int startNodeX     = startPosition.x - clusterPosition.x * graphClusterLength;
            int startNodeY     = startPosition.y - clusterPosition.y * graphClusterLength;
            int endNodeIndex   = endNodeX + endNodeY * graphClusterLength;
            int startNodeIndex = startNodeX + startNodeY * graphClusterLength;

            // Initialize the starting pathNode
            int hCost = ManhattenDistance(graphNodePositions[graphIndexArray[startNodeIndex]], endPosition);
            gCostArray[startNodeIndex] = 0;
            hCostArray[startNodeIndex] = hCost;
            fCostArray[startNodeIndex] = hCost;
            openArray[startNodeIndex]  = true;

            // Add the starting node to the open set
            openSet.Enqueue(startNodeIndex, hCost);

            #endregion
            #region SEARCH GRAPH

            while (openSet.Length > 0)
            {
                // Cache the pathNodeIndex we are working with during this iteration
                int currentNodeIndex =
                    openSet.DequeueMin(localIndexArray, closedArray);

                openArray[currentNodeIndex]   = false;
                closedArray[currentNodeIndex] = true;

                // Break if we reached our goal
                if (currentNodeIndex == endNodeIndex)
                {
                    break;
                }

                int2 currentNodePosition =
                    graphNodePositions[graphIndexArray[currentNodeIndex]];

                for (int nIndex = 0; nIndex < neighbourOffsetArray.Length; nIndex++)
                {
                    int2 neighbourPosition =
                        currentNodePosition + neighbourOffsetArray[nIndex];

                    if (!ValidateGridPosition(neighbourPosition, clusterPosition, graphClusterLength))
                    {
                        continue;
                    }

                    int graphNeighbour =
                        neighbourPosition.x + neighbourPosition.y * graphCellLength;

                    if (graphNodeWalkables[graphNeighbour] > 0)
                    {
                        continue;
                    }

                    // Get the local neighbour index
                    int2 localNeighbourPosition = new int2(
                        neighbourPosition.x - clusterPosition.x * graphClusterLength,
                        neighbourPosition.y - clusterPosition.y * graphClusterLength);
                    int localNeighbourIndex =
                        localNeighbourPosition.x + localNeighbourPosition.y * graphClusterLength;

                    // Skip if its closed (already searched)
                    if (closedArray[localNeighbourIndex])
                    {
                        continue;
                    }

                    // Calculate the cost to move from current node to neighbour node
                    int distanceCost =
                        ManhattenDistance(currentNodePosition, neighbourPosition);
                    int tentativeCost =
                        gCostArray[currentNodeIndex] + distanceCost;

                    if (tentativeCost < gCostArray[localNeighbourIndex])
                    {
                        int newHCost = ManhattenDistance(neighbourPosition, endPosition);

                        parentArray[localNeighbourIndex] = currentNodeIndex;
                        hCostArray[localNeighbourIndex]  = newHCost;
                        gCostArray[localNeighbourIndex]  = tentativeCost;
                        fCostArray[localNeighbourIndex]  = tentativeCost + hCost;

                        if (!openArray[localNeighbourIndex])
                        {
                            openArray[localNeighbourIndex] = true;
                            openSet.Enqueue(localNeighbourIndex, fCostArray[localNeighbourIndex]);
                        }
                    }
                }
            }

            #endregion
            #region TRACE PATH

            NativeList <int2> path         = new NativeList <int2>(Allocator.Temp);
            NativeList <int2> smoothedPath = new NativeList <int2>(Allocator.Temp);
            int nodeIndex = endNodeIndex;

            if (parentArray[endNodeIndex] == -1)
            {
                openSet.Dispose();
                localIndexArray.Dispose();
                graphIndexArray.Dispose();
                parentArray.Dispose();
                hCostArray.Dispose();
                gCostArray.Dispose();
                fCostArray.Dispose();
                openArray.Dispose();
                closedArray.Dispose();
                return(path);
            }
            else
            {
                smoothedPath.Add(endPosition);
                path.Add(endPosition);

                while (parentArray[nodeIndex] != -1)
                {
                    path.Add(graphNodePositions[graphIndexArray[parentArray[nodeIndex]]]);
                    nodeIndex = parentArray[nodeIndex];
                }
            }

            #endregion
            #region SMOOTH PATH

            if (path.Length > 2)   // If its less than or equal 2 theres no need to smooth the path
            {
                int  fromIndex = 0;
                bool foundPath = false;

                while (!foundPath)
                {
                    int currentIndex = fromIndex + 2; // Because the next index is always going to be in line of sight

                    if (currentIndex > path.Length - 1)
                    {
                        break;
                    }

                    while (true)
                    {
                        int stopIndex           = currentIndex - 1;
                        int graphNodeArrayIndex = path[stopIndex].x + path[stopIndex].y * graphCellLength;

                        int2 start = path[fromIndex];
                        int2 end   = path[currentIndex];

                        if (!LOSBetween2Nodes(start, end))
                        {
                            smoothedPath.Add(graphNodePositions[graphNodeArrayIndex]);
                            fromIndex = stopIndex;
                            break;
                        }
                        else
                        {
                            if (currentIndex >= path.Length - 1)
                            {
                                foundPath = true;
                                break;
                            }
                            currentIndex++;
                        }
                    }
                }
            }

            #endregion
            #region RETURN

            openSet.Dispose();
            localIndexArray.Dispose();
            graphIndexArray.Dispose();
            parentArray.Dispose();
            hCostArray.Dispose();
            gCostArray.Dispose();
            fCostArray.Dispose();
            openArray.Dispose();
            closedArray.Dispose();
            path.Dispose();
            return(smoothedPath);

            #endregion
        }
コード例 #5
0
        protected override void OnUpdate()
        {
            var mapSettings = GetSingleton <MapSettings>();
            var neighbors   = _neighbors;

            // The data is in not valid state
            if (mapSettings.Tiles.Length < 1)
            {
                return;
            }

            // Other data
            var movementComponents = GetComponentDataFromEntity <MovementCost>(true);
            var tilesSize          = mapSettings.Tiles.Length;
            var commandBuffer      = _eseCommandBufferSystem.CreateCommandBuffer();

            Entities
            .WithoutBurst()
            .WithNone <Waypoint>()
            .ForEach((Entity requestEntity, ref PathRequest pathRequest) =>
            {
                _markerAStar.Begin();

                #region Setup

                _markerSetup.Begin();
                NativeArray <float2> costs  = new NativeArray <float2>(tilesSize, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
                NativeArray <byte> closeSet = new NativeArray <byte>(tilesSize, Allocator.Temp);
                NativeArray <int> camesFrom = new NativeArray <int>(tilesSize, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
                NativeMinHeap minSet        = new NativeMinHeap(tilesSize * 2, Allocator.Temp);

                _markerSetupData.Begin();
                // Setup initial data
                for (int i = 0; i < tilesSize; i++)
                {
                    costs[i] = new float2 {
                        x = 0, y = float.MaxValue
                    };
                    camesFrom[i] = -1;
                }

                _markerSetupData.End();

                // Shortcuts
                int startTile = Index1D(pathRequest.Start, tilesSize);
                int endTile   = Index1D(pathRequest.End, tilesSize);

                costs[startTile] = 0;
                _markerSetup.End();

                #endregion Setup

                #region Search

                _markerSearch.Begin();
                int lastTile = startTile;
                // While not in destination and not at dead end
                while (lastTile != endTile && lastTile != -1)
                {
                    // Mark current tile as visited
                    closeSet[lastTile] = 1;

                    for (int i = 0; i < neighbors.Length; ++i)
                    {
                        var neighbor = neighbors[i];
                        // Find linear neighbor index
                        var neighborIndex = neighbor.Of(lastTile, tilesSize);
                        // Check if neighbor exists
                        if (neighborIndex != -1)
                        {
                            // Previous + current cost
                            _markerMovementData.Begin();
                            var currentCost = movementComponents[mapSettings.Tiles[neighborIndex]];
                            _markerMovementData.End();

                            if (currentCost.Cost == MovementCost.IMPOSSIBLE)
                            {
                                closeSet[neighborIndex] = 0;
                                continue;
                            }

                            var costG = costs[lastTile].x + neighbor.Distance * currentCost.Cost;
                            var costF = costG + Implementation.Heuristic(pathRequest, neighborIndex, tilesSize);

                            if (costs[neighborIndex].y > costF)
                            {
                                // Update cost and path
                                costs[neighborIndex]     = new float2(costG, costF);
                                camesFrom[neighborIndex] = lastTile;

                                // Update min set
                                if (closeSet[neighborIndex] == 0)
                                {
                                    minSet.Push(new MinHeapNode(neighborIndex, costF));
                                }
                            }
                        }
                    }

                    lastTile = Implementation.FindCurrent(minSet, closeSet);
                }
                _markerSearch.End();

                #endregion Search

                #region ReconstructPath

                _markerReconstruct.Begin();
                var waypoints = commandBuffer.AddBuffer <Waypoint>(requestEntity);
                // Travel back through path
                while (lastTile != -1)
                {
                    waypoints.Add(new Waypoint()
                    {
                        Position = Index2D(lastTile, tilesSize)
                    });
                    lastTile = camesFrom[lastTile];
                }
                _markerReconstruct.End();

                #endregion ReconstructPath

                commandBuffer.RemoveComponent <PathRequest>(requestEntity);

                #region Cleanup

                _markerCleanup.Begin();
                // Dispose all temporary data
                costs.Dispose();
                closeSet.Dispose();
                camesFrom.Dispose();
                minSet.Dispose();
                _markerCleanup.End();

                #endregion Cleanup

                _markerAStar.End();
            }).Run();
        }
コード例 #6
0
            public void Execute(Entity entity, int index, ref PathFindingRequest request)
            {
                _iterations    = 0;
                _pathNodeCount = 0;

                //Generate Working Containers
                var openSet   = new NativeMinHeap(MapSize, Allocator.Temp);
                var cameFrom  = new NativeArray <int>(MapSize, Allocator.Temp);
                var costCount = new NativeArray <int>(MapSize, Allocator.Temp);

                for (var i = 0; i < MapSize; i++)
                {
                    costCount[i] = int.MaxValue;
                }

                // Path finding
                var startId = request.StartId;
                var goalId  = request.GoalId;

                openSet.Push(new MinHeapNode(startId, 0));
                costCount[startId] = 0;

                var currentId = -1;

                while (_iterations < IterationLimit && openSet.HasNext())
                {
                    var currentNode = openSet[openSet.Pop()];
                    currentId = currentNode.Id;
                    if (currentId == goalId)
                    {
                        break;
                    }

                    var neighboursId = new NativeList <int>(4, Allocator.Temp);
                    Nodes[currentId].GetNeighbours(ref neighboursId);

                    foreach (var neighbourId in neighboursId)
                    {
                        //if cost == -1 means obstacle, skip
                        if (Nodes[neighbourId].GetCost() == -1)
                        {
                            continue;
                        }

                        var currentCost = costCount[currentId] == int.MaxValue
                            ? 0
                            : costCount[currentId];
                        var newCost = currentCost + Nodes[neighbourId].GetCost();
                        //not better, skip
                        if (costCount[neighbourId] <= newCost)
                        {
                            continue;
                        }

                        var priority = newCost + Nodes[neighbourId].Heuristic(goalId);
                        openSet.Push(new MinHeapNode(neighbourId, priority));
                        cameFrom[neighbourId]  = currentId;
                        costCount[neighbourId] = newCost;
                    }

                    _iterations++;
                    neighboursId.Dispose();
                }

                //Construct path
                var buffer = ResultECB.AddBuffer <PathRoute>(index, entity);
                var nodeId = goalId;

                while (_pathNodeCount < PathNodeLimit && !nodeId.Equals(startId))
                {
                    buffer.Add(new PathRoute {
                        Id = nodeId
                    });
                    nodeId = cameFrom[nodeId];
                    _pathNodeCount++;
                }

                //Construct Result
                var success = true;
                var log     = new NativeString64("Path finding success");

                if (!openSet.HasNext() && currentId != goalId)
                {
                    success = false;
                    log     = new NativeString64("Out of openset");
                }
                if (_iterations >= IterationLimit && currentId != goalId)
                {
                    success = false;
                    log     = new NativeString64("Iteration limit reached");
                }
                else if (_pathNodeCount >= PathNodeLimit && !nodeId.Equals(startId))
                {
                    success = false;
                    log     = new NativeString64("Step limit reached");
                }
                ResultECB.AddComponent(index, entity, new PathResult
                {
                    Success = success,
                    Log     = log
                });

                //Clean result at end of simulation
                CleanECB.DestroyEntity(index, entity);

                //Clear
                openSet.Dispose();
                cameFrom.Dispose();
                costCount.Dispose();
            }
コード例 #7
0
            private void FindPathUsingAStar(int startNavUnit, int endNavUnit, NativeList <int> path)
            {
                if (startNavUnit < 0 || endNavUnit < 0 ||
                    !navUnits[startNavUnit].IsNavigable() || !navUnits[endNavUnit].IsNavigable() ||
                    startNavUnit == endNavUnit)
                {
                    return;
                }

                Bounds endNavUnitBounds = navUnits[endNavUnit].GetRelativeBounds();

                // SimplePriorityQueue<int> openList = new SimplePriorityQueue<int>();
                // HashSet<int> closedList = new HashSet<int>();

                NativeMinHeap <int> openList   = new NativeMinHeap <int>(Allocator.Temp);
                NativeArray <bool>  closedList = new NativeArray <bool>(navUnits.Length, Allocator.Temp);

                int l = 0;

                for (int j = 0; j < navGridSizeY; j++)
                {
                    for (int k = 0; k < navGridSizeZ; k++)
                    {
                        for (int i = 0; i < navGridSizeX; i++)
                        {
                            navUnits[l] = navUnits[l].ResetPathFindingData();
                            l++;
                        }
                    }
                }

                openList.Push(startNavUnit, navUnits[startNavUnit].AStarData.F);

                while (openList.Size() > 0)
                {
                    int  curNavUnit = openList.Pop();
                    int3 curNavPos  = GetPosFromIndex(curNavUnit);

                    Bounds curNavUnitBounds = navUnits[curNavUnit].GetRelativeBounds();

                    for (int i = 0; i < neighborOffsets.Length; i++)
                    {
                        int3 neighborOffset = neighborOffsets[i];

                        int neighbor = GetIndexFromPos(curNavPos.x + neighborOffset.x, curNavPos.y + neighborOffset.y,
                                                       curNavPos.z + neighborOffset.z);

                        if (neighbor < 0 || !navUnits[neighbor].IsNavigable())
                        {
                            continue;
                        }

                        if (neighbor == endNavUnit)
                        {
                            navUnits[neighbor] = navUnits[neighbor].SetPathFindingParentIndex(curNavUnit);

                            BackTracePath(endNavUnit, path);
                            return;
                        }

                        if (!closedList[neighbor])
                        {
                            Bounds neighborBounds = navUnits[neighbor].GetRelativeBounds();

                            float newG = navUnits[curNavUnit].AStarData.G +
                                         Vector3.Distance(curNavUnitBounds.center, neighborBounds.center);
                            float newH = Vector3.Distance(neighborBounds.center, endNavUnitBounds.center);
                            float newF = newG + newH;

                            if (newF < navUnits[neighbor].AStarData.F)
                            {
                                navUnits[neighbor] =
                                    navUnits[neighbor].UpdatePathFindingValues(newF, newG, newH, curNavUnit);

                                openList.Push(neighbor, navUnits[neighbor].AStarData.F);
                            }
                        }
                    }

                    closedList[curNavUnit] = true;
                }

                openList.Dispose();
                closedList.Dispose();
            }
        private NativeList <AbstractNode> FindAbstractPathInGraph(int2 startEdgesKey, int2 startPosition, int2 endPosition)
        {
            #region DATA SETUP

            int EDGE_ARRAY_SIZE = graphIntraEdges.Length;

            // Node sets
            NativeMinHeap openSet = new NativeMinHeap();
            openSet.Initialize(EDGE_ARRAY_SIZE, -1, EDGE_ARRAY_SIZE + 10);
            NativeArray <int>  parentEdgeArray      = new NativeArray <int>(EDGE_ARRAY_SIZE, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
            NativeArray <int>  parentPathIndexArray = new NativeArray <int>(EDGE_ARRAY_SIZE, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
            NativeArray <int>  hCostArray           = new NativeArray <int>(EDGE_ARRAY_SIZE, Allocator.Temp);
            NativeArray <int>  gCostArray           = new NativeArray <int>(EDGE_ARRAY_SIZE, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
            NativeArray <int>  fCostArray           = new NativeArray <int>(EDGE_ARRAY_SIZE, Allocator.Temp);
            NativeArray <bool> openArray            = new NativeArray <bool>(EDGE_ARRAY_SIZE, Allocator.Temp, NativeArrayOptions.ClearMemory);
            NativeArray <bool> closedArray          = new NativeArray <bool>(EDGE_ARRAY_SIZE, Allocator.Temp, NativeArrayOptions.ClearMemory);

            int   intValue = -1;
            void *pointer  = ( void * )&intValue;
            UnsafeUtility.MemCpyReplicate(parentEdgeArray.GetUnsafePtr(), pointer, sizeof(int), EDGE_ARRAY_SIZE);
            UnsafeUtility.MemCpyReplicate(parentPathIndexArray.GetUnsafePtr(), pointer, sizeof(int), EDGE_ARRAY_SIZE);
            intValue = int.MaxValue;
            UnsafeUtility.MemCpyReplicate(gCostArray.GetUnsafePtr(), pointer, sizeof(int), EDGE_ARRAY_SIZE);

            // Initialize the starting nodes(edges) from the parameter
            for (int i = startEdgesKey.x; i < startEdgesKey.y; i++)
            {
                int nodeIndex = graphIntraEdges[i];
                int edgeIndex = graphNodeToEdge[nodeIndex];

                hCostArray[edgeIndex] = ManhattenDistance(graphNodePositions[nodeIndex], endPosition);
                gCostArray[edgeIndex] = ManhattenDistance(startPosition, graphNodePositions[nodeIndex]);
                fCostArray[edgeIndex] = gCostArray[edgeIndex] + hCostArray[edgeIndex];

                openSet.Enqueue(edgeIndex, fCostArray[edgeIndex]);
            }

            // So we can test when we have reached the end
            int2 endClusterPosition = new int2(
                endPosition.x / graphClusterSize,
                endPosition.y / graphClusterSize);

            int endEdgeIndex = -1;

            #endregion
            #region FIND PATH

            while (openSet.Length > 0)
            {
                int currentEdge      = openSet.DequeueMin(closedArray);
                int currentInterNode = graphInterEdges[currentEdge];   // graphNodeInterIndexArray[ currentEdge ];

                int2 clusterPosition = new int2(
                    graphNodePositions[currentInterNode].x / graphClusterSize,
                    graphNodePositions[currentInterNode].y / graphClusterSize);

                if (clusterPosition.Equals(endClusterPosition))
                {
                    endEdgeIndex = currentEdge;
                    break;
                }

                openArray[currentEdge]   = false;
                closedArray[currentEdge] = true;

                int2 interNodeNeighbours = graphEdgeNeighbourList[currentInterNode];
                for (int i = interNodeNeighbours.x; i < interNodeNeighbours.y; i++)
                {
                    int neighbourNode = graphEdgeNeighbours[i];
                    int neighbourEdge = graphNodeToEdge[neighbourNode];

                    if (closedArray[neighbourEdge])
                    {
                        continue;
                    }

                    int distanceCost = ManhattenDistance(
                        graphNodePositions[currentInterNode], graphNodePositions[neighbourNode]);
                    int tentativeCost =
                        distanceCost + gCostArray[currentEdge];

                    if (tentativeCost < gCostArray[neighbourEdge])
                    {
                        int newHCost = ManhattenDistance(graphNodePositions[neighbourNode], endPosition);
                        parentEdgeArray[neighbourEdge]      = currentEdge;
                        parentPathIndexArray[neighbourEdge] = i;
                        hCostArray[neighbourEdge]           = newHCost;
                        gCostArray[neighbourEdge]           = tentativeCost;
                        fCostArray[neighbourEdge]           = newHCost + tentativeCost;

                        if (!openArray[neighbourEdge])
                        {
                            openArray[neighbourEdge] = true;
                            openSet.Enqueue(neighbourEdge, fCostArray[neighbourEdge]);
                        }
                    }
                }
            }

            #endregion
            #region TRACE PATH

            NativeList <AbstractNode> pathNodes = new NativeList <AbstractNode>(Allocator.Temp);

            if (endEdgeIndex != -1)
            {
                // Add the end position and the inter-node it connects to
                int interNodeIndex = graphInterEdges[endEdgeIndex];
                pathNodes.Add(new AbstractNode
                {
                    position = endPosition,
                    path     = 0
                });
                pathNodes.Add(new AbstractNode
                {
                    position = graphNodePositions[interNodeIndex],
                    path     = 0
                });

                // Trace the path
                int edgeIndex = endEdgeIndex;
                while (parentEdgeArray[edgeIndex] != -1)
                {
                    AbstractNode node = new AbstractNode();
                    node.position = graphNodePositions[graphIntraEdges[edgeIndex]];
                    node.path     = parentPathIndexArray[edgeIndex];

                    pathNodes.Add(node);
                    edgeIndex = parentEdgeArray[edgeIndex];
                }

                // Add the internode of the first edge to the path and the starting position
                int interEdge = graphNodeToEdge[graphInterEdges[edgeIndex]];
                int interNode = graphInterEdges[interEdge];

                pathNodes.Add(new AbstractNode
                {
                    position = graphNodePositions[interNode],
                    path     = 0
                });
                pathNodes.Add(new AbstractNode
                {
                    position = startPosition,
                    path     = 0
                });

                openSet.Dispose();
                parentEdgeArray.Dispose();
                parentPathIndexArray.Dispose();
                hCostArray.Dispose();
                gCostArray.Dispose();
                fCostArray.Dispose();
                openArray.Dispose();
                closedArray.Dispose();

                return(pathNodes);
            }
            else
            {
                openSet.Dispose();
                parentEdgeArray.Dispose();
                parentPathIndexArray.Dispose();
                hCostArray.Dispose();
                gCostArray.Dispose();
                fCostArray.Dispose();
                openArray.Dispose();
                closedArray.Dispose();

                return(pathNodes);
            }

            #endregion
        }
        private FixedList4096 <int2> FindLowLevelPathInCluster(int2 startPosition, int2 endPosition, int2 clusterPosition)
        {
            #region DATA SETUP

            int PATH_NODE_ARRAY_SIZE = graphClusterSize * graphClusterSize;

            // Node sets
            NativeMinHeap openSet = new NativeMinHeap();
            openSet.Initialize(PATH_NODE_ARRAY_SIZE, -1, PATH_NODE_ARRAY_SIZE + 1);
            NativeArray <int>  graphIndexArray = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
            NativeArray <int>  parentArray     = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
            NativeArray <int>  hCostArray      = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp);
            NativeArray <int>  gCostArray      = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
            NativeArray <int>  fCostArray      = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp);
            NativeArray <bool> openArray       = new NativeArray <bool>(PATH_NODE_ARRAY_SIZE, Allocator.Temp, NativeArrayOptions.ClearMemory);
            NativeArray <bool> closedArray     = new NativeArray <bool>(PATH_NODE_ARRAY_SIZE, Allocator.Temp, NativeArrayOptions.ClearMemory);

            int   intValue = -1;
            void *pointer  = ( void * )&intValue;
            UnsafeUtility.MemCpyReplicate(parentArray.GetUnsafePtr(), pointer, sizeof(int), PATH_NODE_ARRAY_SIZE);
            intValue = int.MaxValue;
            UnsafeUtility.MemCpyReplicate(gCostArray.GetUnsafePtr(), pointer, sizeof(int), PATH_NODE_ARRAY_SIZE);

            /*for ( int localRow = 0; localRow < graphClusterSize; localRow++ )
             * {
             *  for ( int localCol = 0; localCol < graphClusterSize; localCol++ )
             *  {
             *      int2 localPos = new int2( localCol , localRow );
             *      int2 graphPos = localPos + graphClusterPos;
             *
             *      int localIndex = localCol + localRow * graphClusterSize;
             *      int graphArrayIndex = graphPos.x + graphPos.y * graphCellLength;
             *
             *      graphIndexArray[ localIndex ] = graphArrayIndex;
             *  }
             * }*/
            int2 graphClusterPos  = graphClusterSize * clusterPosition;
            int4 graphClusterX    = new int4(clusterPosition.x * graphClusterSize);
            int4 graphClusterY    = new int4(clusterPosition.y * graphClusterSize);
            int4 indexOffest      = new int4(0, 1, 2, 3);
            int  vectorLoopLength = graphClusterSize - 4;
            for (int localRow = 0; localRow < graphClusterSize; localRow++)
            {
                int localCol = 0;

                for ( ; localCol < vectorLoopLength; localCol += 4)
                {
                    int  localIndex      = localCol + localRow * graphClusterSize;
                    int4 localCol4       = new int4(localCol);
                    int4 localRow4       = new int4(localRow);
                    int4 graphX          = localCol4 + indexOffest + graphClusterX;
                    int4 graphY          = localRow4 + graphClusterY;
                    int4 graphArrayIndex = graphX + graphY * graphCellLength;
                    graphIndexArray.ReinterpretStore <int4>(localIndex, graphArrayIndex);
                }
                for ( ; localCol < graphClusterSize; localCol++)
                {
                    int2 localPos = new int2(localCol, localRow);
                    int2 graphPos = localPos + graphClusterPos;

                    int localIndex      = localCol + localRow * graphClusterSize;
                    int graphArrayIndex = graphPos.x + graphPos.y * graphCellLength;

                    graphIndexArray[localIndex] = graphArrayIndex;
                }
            }

            // Get and cache the start and end pathNodeIndices
            int2 clusterNodePos = clusterPosition * graphClusterSize;
            int2 endNodePos     = endPosition - clusterNodePos;
            int2 startNodePos   = startPosition - clusterNodePos;
            int  endNodeIndex   = endNodePos.x + endNodePos.y * graphClusterSize;
            int  startNodeIndex = startNodePos.x + startNodePos.y * graphClusterSize;

            // Initialize the starting pathNode
            int hCost = ManhattenDistance(graphNodePositions[graphIndexArray[startNodeIndex]], endPosition);
            gCostArray[startNodeIndex] = 0;
            hCostArray[startNodeIndex] = hCost;
            fCostArray[startNodeIndex] = hCost;
            openArray[startNodeIndex]  = true;

            // Add the starting node to the open set
            openSet.Enqueue(startNodeIndex, hCost);

            #endregion
            #region SEARCH GRAPH

            while (openSet.Length > 0)
            {
                // Cache the pathNodeIndex we are working with during this iteration
                int currentNodeIndex = openSet.DequeueMin(closedArray);

                openArray[currentNodeIndex]   = false;
                closedArray[currentNodeIndex] = true;

                // Break if we reached our goal
                if (currentNodeIndex == endNodeIndex)
                {
                    break;
                }

                int2 currentNodePosition = graphNodePositions[graphIndexArray[currentNodeIndex]];
                int2 neighbourKey        = graphNodeNeighbourKeys[graphIndexArray[currentNodeIndex]];
                int  i            = neighbourKey.x;
                int  vectorLength = neighbourKey.y - 4;

                /*for ( ; i < neighbourKey.y; i++ )
                 * {
                 *  int2 neighbourPosition = graphNodePositions[ graphNodeNeighbours[ i ] ];
                 *  int2 localNeighbourPosition = neighbourPosition - clusterPosition * graphClusterSize;
                 *  int localNeighbourIndex = localNeighbourPosition.x + localNeighbourPosition.y * graphClusterSize;
                 *
                 *  // Skip if its closed (already searched)
                 *  if ( closedArray[ localNeighbourIndex ] )
                 *      continue;
                 *
                 *  // Calculate the cost to move from current node to neighbour node
                 *  int distanceCost = ManhattenDistance( currentNodePosition , neighbourPosition );
                 *  int tentativeCost = gCostArray[ currentNodeIndex ] + distanceCost;
                 *
                 *  if ( tentativeCost < gCostArray[ localNeighbourIndex ] )
                 *  {
                 *      int newHCost = ManhattenDistance( neighbourPosition , endPosition );
                 *
                 *      parentArray[ localNeighbourIndex ] = currentNodeIndex;
                 *      hCostArray[ localNeighbourIndex ] = newHCost;
                 *      gCostArray[ localNeighbourIndex ] = tentativeCost;
                 *      fCostArray[ localNeighbourIndex ] = tentativeCost + hCost;
                 *
                 *      if ( !openArray[ localNeighbourIndex ] )
                 *      {
                 *          openArray[ localNeighbourIndex ] = true;
                 *          openSet.Enqueue( localNeighbourIndex , fCostArray[ localNeighbourIndex ] );
                 *      }
                 *  }
                 * }*/

                /*int4 clusterXScaled = new int4( clusterPosition.x * graphClusterSize );
                 * int4 clusterYScaled = new int4( clusterPosition.y * graphClusterSize );
                 *
                 * for ( ; i < vectorLength; i += 4 )
                 * {
                 *  int2x4 neighbourPosition = graphNodePositions.ReinterpretLoad<int2x4>( graphNodeNeighbours[ i ] );
                 *  int4 neighbourX = new int4( neighbourPosition.c0.x , neighbourPosition.c1.x , neighbourPosition.c2.x , neighbourPosition.c3.x );
                 *  int4 neighbourY = new int4( neighbourPosition.c0.y , neighbourPosition.c1.y , neighbourPosition.c2.y , neighbourPosition.c3.y );
                 *
                 *  int4 localNeighbourX = neighbourX - clusterXScaled;
                 *  int4 localNeighbourY = neighbourY - clusterYScaled;
                 *  int4 localNeighbourIndex = localNeighbourX + localNeighbourY * graphClusterSize;
                 *
                 *  int4 closedArrayBranch = math.select( new int4( 1 ) , new int4( 0 ) , closedArray.ReinterpretLoad<bool4>( localNeighbourIndex.x ) );
                 *
                 *  int4 differenceX = neighbourX - new int4( currentNodePosition.x );
                 *  int4 differenceY = neighbourY - new int4( currentNodePosition.y );
                 *
                 *  int4 distanceCost = math.abs( differenceX ) + math.abs( differenceY );
                 *  int4 tentativeCost = new int4( gCostArray[ currentNodeIndex ] + distanceCost );
                 *
                 *  int4 gcost = gCostArray.ReinterpretLoad<int4>( localNeighbourIndex.x ) * closedArrayBranch;
                 *  bool4 lesser = tentativeCost < gcost;
                 *
                 *  if ( lesser.x )
                 *  {
                 *      int newHCost = ManhattenDistance( neighbourPosition.c0 , endPosition );
                 *
                 *      parentArray[ localNeighbourIndex.x ] = currentNodeIndex;
                 *      hCostArray[ localNeighbourIndex.x ] = newHCost;
                 *      gCostArray[ localNeighbourIndex.x ] = tentativeCost.x;
                 *      fCostArray[ localNeighbourIndex.x ] = tentativeCost.x + hCost;
                 *
                 *      if ( !openArray[ localNeighbourIndex.x ] )
                 *      {
                 *          openArray[ localNeighbourIndex.x ] = true;
                 *          openSet.Enqueue( localNeighbourIndex.x , fCostArray[ localNeighbourIndex.x ] );
                 *      }
                 *  }
                 *  if ( lesser.y )
                 *  {
                 *      int newHCost = ManhattenDistance( neighbourPosition.c1 , endPosition );
                 *
                 *      parentArray[ localNeighbourIndex.y ] = currentNodeIndex;
                 *      hCostArray[ localNeighbourIndex.y ] = newHCost;
                 *      gCostArray[ localNeighbourIndex.y ] = tentativeCost.y;
                 *      fCostArray[ localNeighbourIndex.y ] = tentativeCost.y + hCost;
                 *
                 *      if ( !openArray[ localNeighbourIndex.y ] )
                 *      {
                 *          openArray[ localNeighbourIndex.y ] = true;
                 *          openSet.Enqueue( localNeighbourIndex.y , fCostArray[ localNeighbourIndex.y ] );
                 *      }
                 *  }
                 *  if ( lesser.z )
                 *  {
                 *      int newHCost = ManhattenDistance( neighbourPosition.c2 , endPosition );
                 *
                 *      parentArray[ localNeighbourIndex.z ] = currentNodeIndex;
                 *      hCostArray[ localNeighbourIndex.z ] = newHCost;
                 *      gCostArray[ localNeighbourIndex.z ] = tentativeCost.z;
                 *      fCostArray[ localNeighbourIndex.z ] = tentativeCost.z + hCost;
                 *
                 *      if ( !openArray[ localNeighbourIndex.z ] )
                 *      {
                 *          openArray[ localNeighbourIndex.z ] = true;
                 *          openSet.Enqueue( localNeighbourIndex.z , fCostArray[ localNeighbourIndex.z ] );
                 *      }
                 *  }
                 *  if ( lesser.w )
                 *  {
                 *      int newHCost = ManhattenDistance( neighbourPosition.c3 , endPosition );
                 *
                 *      parentArray[ localNeighbourIndex.w ] = currentNodeIndex;
                 *      hCostArray[ localNeighbourIndex.w ] = newHCost;
                 *      gCostArray[ localNeighbourIndex.w ] = tentativeCost.w;
                 *      fCostArray[ localNeighbourIndex.w ] = tentativeCost.w + hCost;
                 *
                 *      if ( !openArray[ localNeighbourIndex.w ] )
                 *      {
                 *          openArray[ localNeighbourIndex.w ] = true;
                 *          openSet.Enqueue( localNeighbourIndex.w , fCostArray[ localNeighbourIndex.w ] );
                 *      }
                 *  }
                 * }*/

                for ( ; i < neighbourKey.y; i++)
                {
                    int2 neighbourPosition      = graphNodePositions[graphNodeNeighbours[i]];
                    int2 localNeighbourPosition = neighbourPosition - clusterPosition * graphClusterSize;
                    int  localNeighbourIndex    = localNeighbourPosition.x + localNeighbourPosition.y * graphClusterSize;

                    // Skip if its closed (already searched)
                    if (closedArray[localNeighbourIndex])
                    {
                        continue;
                    }

                    // Calculate the cost to move from current node to neighbour node
                    int distanceCost  = ManhattenDistance(currentNodePosition, neighbourPosition);
                    int tentativeCost = gCostArray[currentNodeIndex] + distanceCost;

                    if (tentativeCost < gCostArray[localNeighbourIndex])
                    {
                        int newHCost = ManhattenDistance(neighbourPosition, endPosition);

                        parentArray[localNeighbourIndex] = currentNodeIndex;
                        hCostArray[localNeighbourIndex]  = newHCost;
                        gCostArray[localNeighbourIndex]  = tentativeCost;
                        fCostArray[localNeighbourIndex]  = tentativeCost + newHCost;

                        if (!openArray[localNeighbourIndex])
                        {
                            openArray[localNeighbourIndex] = true;
                            openSet.Enqueue(localNeighbourIndex, fCostArray[localNeighbourIndex]);
                        }
                    }
                }
            }

            #endregion
            #region TRACE PATH

            NativeList <int2>    path         = new NativeList <int2>(Allocator.Temp);
            FixedList4096 <int2> smoothedPath = new FixedList4096 <int2>(); //Allocator.Temp );

            int nodeIndex = endNodeIndex;

            if (parentArray[endNodeIndex] == -1)
            {
                openSet.Dispose();
                graphIndexArray.Dispose();
                parentArray.Dispose();
                hCostArray.Dispose();
                gCostArray.Dispose();
                fCostArray.Dispose();
                openArray.Dispose();
                closedArray.Dispose();
                return(smoothedPath);
            }
            else
            {
                smoothedPath.Add(endPosition);
                path.Add(endPosition);

                while (parentArray[nodeIndex] != -1)
                {
                    path.Add(graphNodePositions[graphIndexArray[parentArray[nodeIndex]]]);
                    nodeIndex = parentArray[nodeIndex];
                }
            }

            #endregion
            #region SMOOTH PATH

            if (path.Length > 2)   // If its less than or equal 2 theres no need to smooth the path
            {
                int fromIndex    = 0;
                int currentIndex = fromIndex + 2;
                int stopIndex    = path.Length - 1;

                while (currentIndex <= stopIndex)
                {
                    int2 start = path[fromIndex];
                    int2 end   = path[currentIndex];

                    if (!LOS_Node(start, end))
                    {
                        int nextFromIndex       = currentIndex - 1;
                        int graphNodeArrayIndex = path[nextFromIndex].x + path[nextFromIndex].y * graphCellLength;

                        smoothedPath.Add(graphNodePositions[graphNodeArrayIndex]);
                        fromIndex = nextFromIndex;
                    }

                    currentIndex++;
                }
            }

            #endregion
            #region RETURN

            openSet.Dispose();
            graphIndexArray.Dispose();
            parentArray.Dispose();
            hCostArray.Dispose();
            gCostArray.Dispose();
            fCostArray.Dispose();
            openArray.Dispose();
            closedArray.Dispose();
            path.Dispose();
            return(smoothedPath);

            #endregion
        }