Ejemplo n.º 1
0
        public static HashSet <CellsPatch> GenerateCellsPatches(IEnumerable <Cell> cells, MovementFlag movement_flags, bool skip_disabled = false)
        {
            var patches = new HashSet <CellsPatch>();

            //create cells patch for each interconnected group of cells
            var cells_copy = new HashSet <Cell>(cells.Where(x => x.HasFlags(movement_flags) && (!skip_disabled || !x.Disabled)));

            while (cells_copy.Count > 0)
            {
                var start_cell = cells_copy.FirstOrDefault();

                if (start_cell == null)
                {
                    break;
                }

                PatchVisitor   patchVisitor = new PatchVisitor();
                HashSet <Cell> visited      = new HashSet <Cell>();

                Algorihms.Visit(start_cell, ref visited, movement_flags, !skip_disabled, visitor: patchVisitor);

                patches.Add(new CellsPatch(patchVisitor.cells, patchVisitor.cellsGrid, movement_flags));
                cells_copy.ExceptWith(visited);
            }

            return(patches);
        }
Ejemplo n.º 2
0
        public bool FindPath(Vec3 from, Vec3 to, MovementFlag flags, ref List <Vec3> path, float merge_distance = -1, bool as_close_as_possible = false, bool include_from = false, float random_coeff = 0, bool bounce = false, float shift_nodes_distance = 0, bool smoothen = true)
        {
            using (m_Navmesh.AcquireReadDataLock())
            {
                List <path_pos> tmp_path = new List <path_pos>();

                if (from.IsEmpty || to.IsEmpty)
                {
                    return(false);
                }

                Cell start = null;
                Cell end   = null;

                bool start_on_nav_mesh = m_Navmesh.GetCellContaining(from, out start, flags, false, as_close_as_possible, -1, false, 2, null);
                bool end_on_nav_mesh   = m_Navmesh.GetCellContaining(to, out end, flags, false, as_close_as_possible, -1, false, 2, null);

                if (bounce)
                {
                    Vec3 bounce_dir = start.AABB.GetBounceDir2D(from);
                    Vec3 new_from   = from + bounce_dir * 10;
                    m_Navmesh.GetCellContaining(new_from, out start, flags, false, as_close_as_possible, -1, false, 2, null);

                    if (!Algorihms.FindPath <Cell>(start, ref end, new_from, to, flags, ref tmp_path, random_coeff, true))
                    {
                        return(false);
                    }

                    tmp_path.Insert(0, new path_pos(start.AABB.Align(from), start));
                }
                else
                {
                    if (!Algorihms.FindPath <Cell>(start, ref end, from, to, flags, ref tmp_path, random_coeff, true))
                    {
                        return(false);
                    }
                }

                if (smoothen && random_coeff == 0)
                {
                    SmoothenPath(ref tmp_path, flags, bounce ? 1 : 0);
                }

                if (DistToKeepFromEdge > 0 && random_coeff == 0)
                {
                    KeepAwayFromEdges(ref tmp_path, flags);
                }

                path = tmp_path.Select(x => x.pos).ToList();

                PostProcessPath(ref path, merge_distance, shift_nodes_distance);

                if (!include_from && start_on_nav_mesh)
                {
                    path.RemoveAt(0);
                }

                return(true);
            }
        }
Ejemplo n.º 3
0
        protected HashSet <ExploreCell> GetUnexploredCells(ExploreCell origin_cell)
        {
            using (new ReadLock(DataLock))
            {
                var agent_pos = m_Navigator.CurrentPos;

                // unexplored selector is collecting ALL unexplored cells matching constraints and filter criteria
                UnexploredSelector selector = new UnexploredSelector(ExploreConstraints, ExploreFilter, IgnoreSmall, agent_pos, m_Navmesh);
                Algorihms.Visit <ExploreCell>(origin_cell, MovementFlag.None, -1, null, selector);

                m_CellsToExploreCount = selector.all_cells_count;
                m_ExploredCellsCount  = m_CellsToExploreCount - selector.unexplored_cells.Count;

                // first try all big cells unvisited by anyone (undelayed)
                HashSet <ExploreCell> unexplored_cells_subset = new HashSet <ExploreCell>(selector.unexplored_cells.Where(x => !x.Small && !x.Delayed));

                if (unexplored_cells_subset.Count > 0)
                {
                    return(unexplored_cells_subset);
                }

                // try all big cells unvisited by me
                unexplored_cells_subset = new HashSet <ExploreCell>(selector.unexplored_cells.Where(x => !x.Small));

                if (unexplored_cells_subset.Count > 0)
                {
                    return(unexplored_cells_subset);
                }

                // try all remaining cells
                return(selector.unexplored_cells);
            }
        }
Ejemplo n.º 4
0
        protected HashSet <ExploreCell> GetUnexploredCells(ExploreCell origin_cell)
        {
            using (new ReadLock(DataLock))
            {
                UnexploredSelector selector = new UnexploredSelector();
                Algorihms.Visit <ExploreCell>(origin_cell, MovementFlag.None, -1, null, selector);

                m_CellsToExploreCount = selector.all_cells_count;
                m_ExploredCellsCount  = m_CellsToExploreCount - selector.unexplored_cells.Count;

                // first try all big cells unvisited by anyone (undelayed)
                HashSet <ExploreCell> unexplored_cells_subset = new HashSet <ExploreCell>(selector.unexplored_cells.Where(x => !x.Small && !x.Delayed));

                if (unexplored_cells_subset.Count > 0)
                {
                    return(unexplored_cells_subset);
                }

                // try all big cells unvisited by me
                unexplored_cells_subset = new HashSet <ExploreCell>(selector.unexplored_cells.Where(x => !x.Small));

                if (unexplored_cells_subset.Count > 0)
                {
                    return(unexplored_cells_subset);
                }

                // try all remaining cells
                return(selector.unexplored_cells);
            }
        }
Ejemplo n.º 5
0
 public bool AreConnected(Vec3 pos1, Vec3 pos2, MovementFlag flags)
 {
     using (new ReadLock(DataLock))
     {
         GridCell pos1_gc = m_GridCells.Find(g => g.Contains2D(pos1));
         GridCell pos2_gc = m_GridCells.Find(g => g.Contains2D(pos2));
         return(Algorihms.AreConnected(pos1_gc, ref pos2_gc, flags));
     }
 }
Ejemplo n.º 6
0
        public bool FindRoughPath(Vec3 from, Vec3 to, ref List <Vec3> path)
        {
            if (from.IsZero() || to.IsZero())
            {
                return(false);
            }

            List <path_pos> tmp_path = new List <path_pos>();

            bool start_on_nav_mesh = GetCellAt(from, out ExploreCell start);
            bool end_on_nav_mesh   = GetCellAt(to, out ExploreCell end);

            using (new ReadLock(DataLock))
                Algorihms.FindPath(start, from, new Algorihms.DestinationPathFindStrategy <ExploreCell>(to, end), MovementFlag.None, ref tmp_path, use_cell_centers: true);

            path = tmp_path.Select(x => x.pos).ToList();
            return(true);
        }
Ejemplo n.º 7
0
        public static List <CellsPatch> GenerateCellsPatches(List <Cell> cells, MovementFlag movement_flags)
        {
            List <CellsPatch> patches = new List <CellsPatch>();

            //create cells patch for each interconnected group of cells
            HashSet <Cell> cells_copy = new HashSet <Cell>(cells);

            while (cells_copy.Count > 0)
            {
                HashSet <Cell> visited = new HashSet <Cell>();

                Algorihms.Visit(cells_copy.First(), ref visited, movement_flags, true, 1, -1, cells_copy);

                patches.Add(new CellsPatch(visited, movement_flags));

                cells_copy.RemoveWhere(x => visited.Contains(x));
            }

            return(patches);
        }
Ejemplo n.º 8
0
        internal List <Cell> GetCellsWithin(Vec3 p, float radius, MovementFlag flags)
        {
            var result_cells = new List <Cell>();

            if (p.IsZero())
            {
                return(result_cells);
            }

            foreach (var cellsG in CellsGrid)
            {
                if (cellsG.Key.Distance2D(p) > radius)
                {
                    continue;
                }

                result_cells.AddRange(Algorihms.GetCellsWithin(cellsG.Value, p, radius, flags, allow_disabled: true));
            }

            return(result_cells);
        }
Ejemplo n.º 9
0
        private void UpdateGridDestination()
        {
            Vec3       destination          = Vec3.Empty;
            List <int> destination_grids_id = null;

            using (new ReadLock(InputLock))
                destination_grids_id = new List <int>(m_DestinationGridsId);

            if (destination_grids_id.Count > 0)
            {
                using (m_Navmesh.AcquireReadDataLock())
                {
                    GridCell current_grid = m_Navmesh.m_GridCells.FirstOrDefault(g => g.Contains2D(CurrentPos));

                    GridCell destination_grid = m_Navmesh.m_GridCells.FirstOrDefault(g => destination_grids_id.Contains(g.Id) && Algorihms.AreConnected(current_grid, ref g, MovementFlag.None));

                    if (destination_grid != null)
                    {
                        destination = m_Navmesh.GetNearestCell(destination_grid.Cells, destination_grid.Center).Center;
                    }
                }
            }

            if (!destination.IsEmpty)
            {
                SetDestination(destination, DestType.Grid, GridDestPrecision);
            }
        }
Ejemplo n.º 10
0
        // assume Navmesh data lock is aquired
        private int GenerateExploreCells(AABB cell_aabb)
        {
            // should not happen
            //if (m_ExploreCells.Exists(x => x.AABB.Equals(cell_aabb)))
            //    return;

            MovementFlag movement_flags = m_Navigator.MovementFlags;

            //using (new Profiler("[Nav] Nav data generated [%t]"))
            //using (m_Navmesh.AquireReadDataLock())
            {
                List <Cell> cells = new List <Cell>();

                // i'm interested in unique list
                HashSet <int> tmp_overlapping_grid_cells = new HashSet <int>();

                // find all cells inside cell_aabb
                using (m_Navmesh.AcquireReadDataLock())
                {
                    foreach (GridCell grid_cell in m_Navmesh.m_GridCells.Where(x => x.AABB.Overlaps2D(cell_aabb)))
                    {
                        tmp_overlapping_grid_cells.Add(grid_cell.Id);

                        cells.AddRange(grid_cell.GetCells(x => x.HasFlags(movement_flags) && cell_aabb.Overlaps2D(x.AABB), false));
                    }
                }

                // for traversing purposes list will be faster
                List <int> overlapping_grid_cells = tmp_overlapping_grid_cells.ToList();

                //create ExploreCell for each interconnected group of cells
                HashSet <Cell> cells_copy = new HashSet <Cell>(cells);
                int            last_explore_cells_count = m_ExploreCells.Count;

                while (cells_copy.Count > 0)
                {
                    HashSet <Cell> visited = new HashSet <Cell>();

                    using (m_Navmesh.AcquireReadDataLock())
                        Algorihms.Visit(cells_copy.First(), ref visited, movement_flags, true, 1, -1, cells_copy);

                    List <AABB> intersections      = new List <AABB>();
                    AABB        intersections_aabb = new AABB();

                    AABB intersection = default(AABB);

                    foreach (Cell c in visited)
                    {
                        if (cell_aabb.Intersect(c.AABB, ref intersection))
                        {
                            intersections.Add(intersection);
                            intersections_aabb = intersections_aabb.Extend(intersection);
                        }
                    }

                    ExploreCell ex_cell = new ExploreCell(cell_aabb, visited.ToList(), overlapping_grid_cells, m_LastExploreCellId++);
                    Add(ex_cell);

                    ex_cell.Small = (ex_cell.CellsArea < MaxAreaToMarkAsSmall) || (ex_cell.CellsAABB.Dimensions.Max() < MaxDimensionToMarkAsSmall);

                    cells_copy.RemoveWhere(x => visited.Contains(x));
                }

                return(m_ExploreCells.Count - last_explore_cells_count);
            }
        }
Ejemplo n.º 11
0
        private void UpdateGridDestination()
        {
            Vec3 destination = Vec3.Empty;

            if (m_DestinationGridsId.Count > 0)
            {
                using (m_Navmesh.AquireReadDataLock())
                {
                    GridCell current_grid = m_Navmesh.m_GridCells.Find(g => g.Contains2D(CurrentPos));

                    GridCell destination_grid = m_Navmesh.m_GridCells.Find(g => m_DestinationGridsId.Contains(g.Id) && Algorihms.AreConnected(current_grid, ref g, MovementFlag.None));

                    if (destination_grid != null)
                    {
                        destination = m_Navmesh.GetNearestCell(destination_grid.Cells, destination_grid.Center).Center;
                    }
                }
            }

            if (!destination.IsEmpty)
            {
                SetDestination(destination, DestType.Grid);
            }
        }
Ejemplo n.º 12
0
        public static void FindExplorePath2Opt(ExploreCell start_cell, List <ExploreCell> explore_cells, CellsDistancer distances, ref List <ExploreCell> path)
        {
            using (new Profiler($"2-Opt %t"))
            {
                path = path ?? new List <ExploreCell>();
                path.Clear();

                if (start_cell == null)
                {
                    return;
                }

                //List<int> cells_id_in_start_cell_network = distances.GetConnectedTo(start_cell.GlobalId);
                //List<ExploreCell> best_tour_old = explore_cells.FindAll(c => cells_id_in_start_cell_network.Contains(c.GlobalId));

                // reordering cell in breadth-first order seems to help with 2-opt backtracking
                var collect_cells_visitor = new CollectVisitor();
                Algorihms.VisitBreadth(start_cell, visitor: collect_cells_visitor);
                var best_tour = collect_cells_visitor.cells.Select(x => x as ExploreCell).Intersect(explore_cells).ToList();

                best_tour.RemoveAll(c => c.Explored || c.Neighbours.Count == 0);

                if (start_cell.Explored) // re-insert start cell if needed
                {
                    best_tour.Insert(0, start_cell);
                }

                if (best_tour.Count == 1)
                {
                    path.Add(best_tour[0]);
                    return;
                }

                float best_dist = GetTravelDistance(best_tour, distances);


                if (best_tour.Count < 90)
                {
                    // based on http://en.wikipedia.org/wiki/2-opt

                    while (true)
                    {
                        bool better_found = false;

                        for (int i = 1; i < best_tour.Count - 1; ++i)
                        {
                            for (int k = i + 1; k < best_tour.Count; ++k)
                            {
                                float new_dist = Swap2OptDistance(best_tour, distances, i, k);

                                if (new_dist < best_dist)
                                {
                                    Swap2Opt(ref best_tour, i, k);
                                    best_dist = new_dist;

                                    better_found = true;
                                    break;
                                }
                            }

                            if (better_found)
                            {
                                break;
                            }
                        }

                        if (!better_found)
                        {
                            break;
                        }
                    }
                }
                else // greedy
                {
                    // based on http://on-demand.gputechconf.com/gtc/2014/presentations/S4534-high-speed-2-opt-tsp-solver.pdf

                    while (true)
                    {
                        float min_change = 0;

                        int min_i = -1;
                        int min_j = -1;

                        for (int i = 0; i < best_tour.Count - 2; ++i)
                        {
                            for (int j = i + 2; j < best_tour.Count - 1; ++j)
                            {
                                int city_a = best_tour[i].GlobalId;
                                int city_b = best_tour[i + 1].GlobalId;
                                int city_c = best_tour[j].GlobalId;
                                int city_d = best_tour[j + 1].GlobalId;

                                float change = (distances.GetDistance(city_a, city_c) + distances.GetDistance(city_b, city_d)) -
                                               (distances.GetDistance(city_a, city_b) + distances.GetDistance(city_c, city_d));

                                if (change < min_change)
                                {
                                    min_change = change;
                                    min_i      = i + 1;
                                    min_j      = j;
                                }
                            }
                        }

                        if (min_change >= 0)
                        {
                            break;
                        }

                        // apply min_i/min_j move
                        ExploreCell t = best_tour[min_i];
                        best_tour[min_i] = best_tour[min_j];
                        best_tour[min_j] = t;
                    }
                }

                Trace.WriteLine("[FindExplorePath 2-Opt] Final solution distance: " + GetTravelDistance(best_tour, distances));

                foreach (ExploreCell cell in best_tour)
                {
                    path.Add(cell);
                }

                if (start_cell.Explored)
                {
                    path.RemoveAt(0);
                }
            }
        }
Ejemplo n.º 13
0
        private int GenerateExploreCells(AABB cell_aabb)
        {
            // should not happen
            //if (m_ExploreCells.Exists(x => x.AABB.Equals(cell_aabb)))
            //    return;

            MovementFlag movement_flags = Navmesh.Navigator.MovementFlags;

            //using (new Profiler("[Nav] Nav data generated [{t}]"))
            using (Navmesh.AquireReadDataLock())
            {
                List <Cell> cells = new List <Cell>();

                // find all cells inside cell_aabb
                foreach (GridCell grid_cell in Navmesh.m_GridCells)
                {
                    if (!cell_aabb.Overlaps2D(grid_cell.AABB))
                    {
                        continue;
                    }

                    cells.AddRange(grid_cell.Cells.FindAll(x => !x.Replacement && x.HasFlags(movement_flags) && cell_aabb.Overlaps2D(x.AABB)));
                }

                // add all replaced cells overlapped by explore cell
                //cells.AddRange(Navmesh.GetReplacedCells().FindAll(x => x.HasFlags(movement_flags) && cell_aabb.Overlaps2D(x.AABB)));

                //create ExploreCell for each interconnected group of cells
                List <Cell> cells_copy = new List <Cell>(cells);
                int         last_explore_cells_count = m_ExploreCells.Count;

                while (cells_copy.Count > 0)
                {
                    List <Cell> visited = new List <Cell>();

                    Algorihms.Visit(cells_copy[0], ref visited, movement_flags, true, 1, -1, cells_copy);

                    List <AABB> intersections      = new List <AABB>();
                    AABB        intersections_aabb = new AABB();

                    foreach (Cell c in visited)
                    {
                        AABB intersection = cell_aabb.Intersect(c.AABB);

                        intersections.Add(intersection);
                        intersections_aabb.Extend(intersection);
                    }

                    Vec3  nearest_intersection_center = Vec3.Empty;
                    float nearest_intersection_dist   = float.MaxValue;

                    foreach (AABB inter_aabb in intersections)
                    {
                        float dist = inter_aabb.Center.Distance2D(intersections_aabb.Center);

                        if (dist < nearest_intersection_dist)
                        {
                            nearest_intersection_center = inter_aabb.Center;
                            nearest_intersection_dist   = dist;
                        }
                    }

                    ExploreCell ex_cell = new ExploreCell(cell_aabb, visited, nearest_intersection_center, m_LastExploreCellId++);
                    Add(ex_cell);

                    ex_cell.Small = (ex_cell.CellsArea() < MAX_AREA_TO_MARK_AS_SMALL);

                    cells_copy.RemoveAll(x => visited.Contains(x));
                }

                return(m_ExploreCells.Count - last_explore_cells_count);
            }
        }
Ejemplo n.º 14
0
        private void UpdateGridDestination()
        {
            Vec3       destination          = Vec3.ZERO;
            bool       grid_dest_found      = false;
            List <int> destination_grids_id = null;

            using (new ReadLock(InputLock))
                destination_grids_id = new List <int>(m_DestinationGridsId);

            if (destination_grids_id.Count > 0)
            {
                using (m_Navmesh.AcquireReadDataLock())
                {
                    GridCell current_grid = m_Navmesh.m_GridCells.FirstOrDefault(x => x.Contains2D(CurrentPos));

                    GridCell destination_grid = m_Navmesh.m_GridCells.FirstOrDefault(x => destination_grids_id.Contains(x.Id) && Algorihms.AreConnected(current_grid, ref x, MovementFlag.None));

                    if (destination_grid != null)
                    {
                        grid_dest_found = true;
                        destination     = m_Navmesh.GetNearestCell(destination_grid.GetCells(), destination_grid.Center).Center;
                    }
                }
            }

            if (grid_dest_found)
            {
                SetDestination(destination, DestType.Grid, GridDestPrecision);
            }
        }