Exemplo n.º 1
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();
            }