Exemplo n.º 1
0
 internal static int FindCurrent(NativeMinHeap minSet, NativeArray <byte> closeSet)
 {
     _markerFindCurrent.Begin();
     while (minSet.HasNext())
     {
         _markerPop.Begin();
         var next = minSet.Pop();
         _markerPop.End();
         // Check if this is not visited tile
         if (closeSet[next.Position] == 0)
         {
             _markerFindCurrent.End();
             return(next.Position);
         }
     }
     _markerFindCurrent.End();
     return(-1);
 }
Exemplo n.º 2
0
        public void Execute()
        {
            int length  = moveCost.Length;
            int start1d = Index2dTo1d(start, moveCost_width);
            {
                for (int i = _F_.Length - 1; i != -1; i--)
                {
                    _F_[i] = float.MaxValue;
                }
                _F_[start1d] = 0;
            }
            {
                solution[start1d] = start;
            }
            {
                frontier.Push(start);
            }
            {
                visited.TryAdd(start, 0);
            }

            //solve;
            float euclideanMaxLength = EuclideanHeuristicMaxLength(length, moveCost_width);
            int2  node = int2.zero - 1;

            while (frontier.Count != 0 && math.any(node != destination))
            {
                node = frontier.Pop();                //we grab candidate with lowest F so far
                int   node1d = Index2dTo1d(node, moveCost_width);
                float node_f = _F_[node1d];

                //lets check all its neighbours:
                EnumerateNeighbours(neighbours, moveCost_width, moveCost_width, node);
                int neighboursLength = neighbours.Length;
                for (int i = 0; i < neighboursLength; i++)
                {
                    int2 neighbour   = neighbours[i];
                    int  neighbour1d = Index2dTo1d(neighbour, moveCost_width);

                    bool isOrthogonal = math.any(node == neighbour);                    //is relative position orthogonal or diagonal

                    float g = node_f + moveCost[neighbour1d] * (isOrthogonal ? 1f : 1.41421356237f);
                    float h = EuclideanHeuristicNormalized(neighbour, destination, euclideanMaxLength) * heuristic_cost;
                    float f = g + h;

                    //update F when this connection is less costly:
                    if (
                        f < _F_[neighbour1d]
                        )
                    {
                        _F_[neighbour1d]      = f;
                        solution[neighbour1d] = node;
                    }

                    //update frontier:
                    if (visited.TryAdd(neighbour, 0))
                    {
                        frontier.Push(neighbour);
                    }
                }
            }

            //create path:
            BacktrackToPath(solution, moveCost_width, destination, Results);
        }
Exemplo n.º 3
0
        private void FindPath(int2 startPosition, int2 endPosition)
        {
            Debug.Log("Pathfinding to: " + endPosition);
            if (startPosition.Equals(endPosition) || Grid[GetIndex(endPosition)] == -1)
            {
                Debug.LogError("asking to go into an obstacle, or is already there!");
                foundPath = false;

                return;
            }



            PathNode head = new PathNode(startPosition, CalculateDistanceCost(startPosition, endPosition));

            OpenSet.Push(head);



            while (itterationLimit > 0 && OpenSet.HasNext())
            {
                int      currentIndex = OpenSet.Pop();
                PathNode current      = OpenSet[currentIndex];

                int ind = GetIndex(current.Position);

                PathNode cameFromNode = CameFrom[ind];


                if (current.Position.Equals(endPosition))
                {
                    //Found our destination, we will let the cleanup job handle the path reconstruction for now
                    //ReconstructPath(startPosition, endPosition);
                    return;
                }

                float initialCost = CostSoFar[GetIndex(current.Position)];

                PathNode[] neighbourNodes = new PathNode[Neighbours.Length];

                for (int i = 0; i < Neighbours.Length; i++)
                {
                    int2 neighbour = Neighbours[i];
                    int2 position  = current.Position + neighbour;

                    if (position.x < 0 || position.x >= DimX || position.y < 0 || position.y >= DimY)
                    {
                        continue;
                    }

                    int index = GetIndex(position);

                    float cellCost = GetCellCost(currentIndex, index, true);

                    if (float.IsInfinity(cellCost))
                    {
                        current.NextToObstacle = true;



                        continue;
                    }

                    neighbourNodes[i] = new PathNode(position, cellCost);
                }

                if (!cameFromNode.Equals(null) && cameFromNode.NextToObstacle && current.NextToObstacle && IsDiagonal(current.Position, cameFromNode.Position))
                {
                    //In this case, the path came from point that was next to a obstacle, and is moving diagonally towards a point next to an obstacle. so we are assuming they are moving diagonally through the obstacle
                    //TODO: this is not always the case, will need to resolve later
                    continue;
                }

                for (int i = 0; i < neighbourNodes.Length; i++)
                {
                    int2 neighbour = Neighbours[i];

                    PathNode neighbourNode = neighbourNodes[i];

                    int index = GetIndex(neighbourNode.Position);

                    if (neighbourNode.Equals(null))
                    {
                        Debug.Log("neighbour null");
                        continue;
                    }

                    float neighbourCost = 10;

                    if ((math.abs(neighbour.x) + math.abs(neighbour.y)) == 2)
                    {
                        neighbourCost = 14;
                    }

                    float newCost = initialCost + neighbourCost + neighbourNode.ExpectedCost;
                    float oldCost = CostSoFar[index];

                    if (!(oldCost <= 0) && !(newCost < oldCost))
                    {
                        continue;
                    }

                    CostSoFar[index] = newCost;
                    CameFrom[index]  = current;


                    neighbourNode.ExpectedCost = newCost + CalculateDistanceCost(neighbourNode.Position, endPosition);


                    OpenSet.Push(neighbourNode);
                }

                itterationLimit--;
            }

            if (OpenSet.HasNext())
            {
                //We ran out of itterations

                //We will just give out where we stapped at for now
                //TODO: fix this
                var currentIndex = OpenSet.Pop();
                endPosition = OpenSet[currentIndex].Position;
            }
        }
Exemplo n.º 4
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();
            }
Exemplo n.º 5
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();
            }