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