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); }
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); } }
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); } }
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); } }
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)); } }
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); }
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); }
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); }
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); } }
// 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); } }
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); } }
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); } } }
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); } }
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); } }