protected ExploreCell GetCurrentExploreCell()
{
Vec3 curr_pos = Navmesh.Navigator.CurrentPos;
ExploreCell current_explore_cell = m_ExploreCells.Find(x => x.CellsContains2D(curr_pos));
if (current_explore_cell == null)
{
current_explore_cell = m_ExploreCells.Find(x => x.Contains2D(curr_pos));
}
// find nearest explore cell
if (current_explore_cell == null)
{
float min_dist = float.MaxValue;
foreach (ExploreCell explore_cell in m_ExploreCells)
{
float dist = explore_cell.Position.DistanceSqr(curr_pos);
if (dist < min_dist)
{
current_explore_cell = explore_cell;
min_dist = dist;
}
}
}
return(current_explore_cell);
}
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);
}
}
internal virtual Vec3 GetDestinationCellPosition()
{
Vec3 hint_pos = HintPos;
if (!hint_pos.IsEmpty)
{
// find nearest unexplored cell to the hint position
float min_dist = float.MaxValue;
ExploreCell nearest_explore_cell = null;
foreach (ExploreCell explore_cell in m_ExploreCells)
{
if (explore_cell.Explored)
{
continue;
}
float dist = explore_cell.Position.DistanceSqr(hint_pos);
if (dist < min_dist)
{
nearest_explore_cell = explore_cell;
min_dist = dist;
}
}
return(nearest_explore_cell != null ? nearest_explore_cell.Position : Vec3.Empty);
}
return(Vec3.Empty);
}
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);
}
}
internal bool GetCellAt(Vec3 p, out ExploreCell result_cell)
{
using (new ReadLock(DataLock))
{
result_cell = null;
if (p.IsZero())
{
return(false);
}
foreach (var explore_cell in m_ExploreCells)
{
if (explore_cell.Contains2D(p))
{
var cells = explore_cell.GetCellsAt(p, test_2d: true, z_tolerance: 0);
if (cells.Count() > 0)
{
result_cell = explore_cell;
return(true);
}
}
}
return(false);
}
}
protected virtual void OnDeserialize(HashSet <Cell> all_cells, BinaryReader r)
{
using (new WriteLock(DataLock))
using (new WriteLock(InputLock))
{
m_ExploreCells.Clear();
m_Enabled = r.ReadBoolean();
int explore_cells_count = r.ReadInt32();
// pre-allocate explore cells
for (int i = 0; i < explore_cells_count; ++i)
{
ExploreCell explore_cell = new ExploreCell();
explore_cell.GlobalId = r.ReadInt32();
m_ExploreCells.Add(explore_cell);
}
foreach (ExploreCell explore_cell in m_ExploreCells)
{
explore_cell.Deserialize(m_ExploreCells, all_cells, r);
}
ExploreCell.LastExploreCellGlobalId = r.ReadInt32();
m_HintPos = new Vec3(r);
}
}
public static void Render(Nav.ExploreCell cell, float radius, PointF trans, PaintEventArgs e, bool draw_connections, bool draw_id)
{
DrawRectangle(e.Graphics, Pens.Magenta, trans, cell.Min, cell.Max);
//DrawString(e.Graphics, Brushes.Black, trans, cell.Position, Math.Round(cell.CellsArea()).ToString(), 14);
if (cell.Explored)
{
//DrawLine(e.Graphics, explored_pen, trans, cell.Min, cell.Max);
//DrawLine(e.Graphics, explored_pen, trans, new Vec3(cell.Min.X, cell.Max.Y), new Vec3(cell.Max.X, cell.Min.Y));
FillRectangle(e.Graphics, explored_brush, trans, cell.Min, cell.Max);
}
else
{
//DrawCircle(e.Graphics, Pens.Red, trans, cell.Position, radius);
//DrawString(e.Graphics, Brushes.Black, trans, cell.Position, cell.UserData.ToString(), 10);
if (draw_connections)
{
foreach (Nav.Cell.Neighbour neighbour in cell.Neighbours)
{
ExploreCell neighbour_cell = (ExploreCell)neighbour.cell;
DrawLine(e.Graphics, EXPLORE_CELL_CONNECTION_PEN, trans, cell.Center, neighbour.border_point);
}
}
}
if (draw_id)
{
DrawString(e.Graphics, Brushes.Black, trans, cell.Position, cell.GlobalId.ToString() + " (" + cell.Id.ToString() + ")", 10);
}
}
internal virtual void Deserialize(List <Cell> all_cells, BinaryReader r)
{
using (new WriteLock(DataLock))
using (new WriteLock(InputLock))
{
m_ExploreCells.Clear();
m_ExploreCellsDistancer.Clear();
m_Enabled = r.ReadBoolean();
int explore_cells_count = r.ReadInt32();
// pre-allocate explore cells
for (int i = 0; i < explore_cells_count; ++i)
{
ExploreCell explore_cell = new ExploreCell();
explore_cell.GlobalId = r.ReadInt32();
m_ExploreCells.Add(explore_cell);
}
m_ExploreCells.Sort(new Cell.CompareByGlobalId());
foreach (ExploreCell explore_cell in m_ExploreCells)
{
explore_cell.Deserialize(m_ExploreCells, all_cells, r);
}
ExploreCell.LastExploreCellGlobalId = r.ReadInt32();
m_ExploreCellsDistancer.Deserialize(r);
m_HintPos = new Vec3(r);
}
}
protected virtual void OnCellExplored(ExploreCell cell)
{
if (cell == null)
{
return;
}
cell.Explored = true; // this is safe as explore cells cannot be added/removed now
m_Navmesh.Log("[Nav] Explored cell " + cell.GlobalId + " [progress: " + GetExploredPercent() + "%]!");
}
private void Add(ExploreCell explore_cell)
{
using (new WriteLock(DataLock))
{
foreach (ExploreCell e_cell in m_ExploreCells)
{
e_cell.AddNeighbour(explore_cell);
}
m_ExploreCells.Add(explore_cell);
}
}
protected virtual void OnDeserialize(HashSet <Cell> all_cells, Dictionary <int, Cell> id_to_cell, BinaryReader r)
{
m_ExploreCells.Clear();
m_Enabled = r.ReadBoolean();
int explore_cells_count = r.ReadInt32();
// pre-allocate explore cells
for (int i = 0; i < explore_cells_count; ++i)
{
ExploreCell explore_cell = new ExploreCell();
explore_cell.GlobalId = r.ReadInt32();
m_ExploreCells.Add(explore_cell);
}
foreach (ExploreCell explore_cell in m_ExploreCells)
{
explore_cell.Deserialize(m_ExploreCells, all_cells, id_to_cell, r);
}
foreach (ExploreCell explore_cell in m_ExploreCells)
{
OnExploreCellAdded(explore_cell);
}
var dest_cell_global_id = r.ReadInt32();
if (dest_cell_global_id >= 0)
{
m_DestCell = m_ExploreCells.FirstOrDefault(x => x.GlobalId == dest_cell_global_id);
}
ExploreCell.LastExploreCellGlobalId = r.ReadInt32();
m_ExploredCellsCount = r.ReadInt32();
m_CellsToExploreCount = r.ReadInt32();
var explore_constraints_count = r.ReadInt32();
var exConstraints = explore_constraints_count > 0 ? new List <AABB>() : null;
if (exConstraints != null)
{
for (int i = 0; i < explore_constraints_count; ++i)
{
exConstraints.Add(new AABB(r));
}
}
m_ExploreConstraints = exConstraints;
m_HintPos = new Vec3(r);
}
public static void Render(Nav.Navmesh navmesh, Nav.ExploreCell cell, List <Nav.ExploreCell> all_cells, PointF trans, PaintEventArgs e, bool draw_id)
{
foreach (ExploreCell other_cell in all_cells)
{
if (cell.Id == other_cell.Id)
{
continue;
}
DrawLine(e.Graphics, Pens.Gray, trans, cell.Position, other_cell.Position);
//DrawString(e.Graphics, Brushes.Black, trans, (cell.Position + other_cell.Position) * 0.5f, Math.Round(navmesh.Explorator.ExploreDistance(cell, other_cell)).ToString(), 8);
}
}
private void Add(ExploreCell explore_cell)
{
using (m_Navmesh.AcquireReadDataLock())
using (new WriteLock(DataLock))
{
foreach (ExploreCell e_cell in m_ExploreCells)
{
e_cell.AddNeighbour(explore_cell);
}
m_ExploreCells.Add(explore_cell);
}
}
private void Add(ExploreCell explore_cell)
{
using (m_Navmesh.AcquireReadDataLock())
using (new WriteLock(DataLock))
{
foreach (ExploreCell e_cell in m_ExploreCells)
{
e_cell.AddNeighbour(explore_cell);
}
m_ExploreCells.Add(explore_cell);
Interlocked.Exchange(ref m_ForceRefreshExploredPercent, 1);
}
}
private void Add(ExploreCell explore_cell)
{
using (new WriteLock(DataLock))
{
foreach (ExploreCell e_cell in m_ExploreCells)
{
if (e_cell.AddNeighbour(explore_cell))
{
m_ExploreCellsDistancer.Connect(e_cell.GlobalId, explore_cell.GlobalId, e_cell.Position.Distance2D(explore_cell.Position));
}
}
m_ExploreCells.Add(explore_cell);
}
}
public void OnDestinationReached(destination dest)
{
if (dest.type != DestType.Explore)
{
return;
}
ExploreCell dest_cell = dest.user_data as ExploreCell;
if (dest_cell != null)
{
OnCellExplored(dest_cell);
}
SelectNewDestinationCell();
}
public void OnDestinationReached(DestType type, Vec3 dest)
{
if (type != DestType.Explore)
{
return;
}
ExploreCell dest_cell = m_ExploreCells.FirstOrDefault(x => x.Position.Equals(dest));
if (dest_cell != null)
{
OnCellExplored(dest_cell);
}
m_DestCell = GetDestinationCell();
m_Navigator.SetDestination(GetDestinationCellPosition(), DestType.Explore, ExploreDestPrecision);
}
public void OnDestinationReached(DestType type, Vec3 dest)
{
if (type != DestType.Explore)
{
return;
}
ExploreCell dest_cell = m_ExploreCells.Find(x => x.Position.Equals(dest));
if (dest_cell != null)
{
OnCellExplored(dest_cell);
}
Vec3 next_dest = GetDestinationCellPosition();
Navmesh.Navigator.SetDestination(next_dest, DestType.Explore);
}
private void UpdateExploration()
{
Vec3 current_pos = m_Navigator.CurrentPos;
if (current_pos.IsZero())
{
return;
}
if (!IsDataAvailable)
{
m_Navmesh.Log("[Nav] Exploration data unavailable!");
return;
}
if (IsExplored())
{
m_Navmesh.Log("[Nav] Exploration finished!");
m_Navigator.ClearDestination(DestType.Explore);
return;
}
using (new ReadLock(DataLock, true))
{
if (m_Navigator.GetDestinationType() < DestType.Explore)
{
m_DestCell = GetDestinationCell();
m_Navigator.SetDestination(GetDestinationCellPosition(), DestType.Explore, ExploreDestPrecision);
//m_Navmesh.Log("[Nav] Explore dest changed.");
}
{
// mark cells as explored when passing by close enough
ExploreCell current_explore_cell = m_ExploreCells.FirstOrDefault(x => !x.Explored && x.Position.Distance2D(current_pos) < ExploreDestPrecision);
if (current_explore_cell != null)
{
OnCellExplored(current_explore_cell);
}
}
OnUpdateExploration();
}
}
private void SelectNewDestinationCell(ExploreCell curr_explore_cell)
{
//using (new Profiler("SelectNewDestinationCell [%t]"))
{
var prev_dest_cell = m_DestCell;
m_DestCell = GetDestinationCell(curr_explore_cell);
//Trace.WriteLine($"dest explore cell id {m_DestCell?.GlobalId ?? -1} pos {GetDestinationCellPosition()}");
//using (new Profiler("set explore cell pos as dest [%t]"))
if (Enabled)
{
m_Navigator.SetDestination(new destination(GetDestinationCellPosition(), DestType.Explore, ExploreDestPrecision, user_data: m_DestCell, stop: false));
}
// force reevaluation so connectivity check is performed on selected cell (it is cheaper than checking connectivity for all unexplored cells)
m_ValidateDestCell = m_DestCell != prev_dest_cell;
}
}
public void OnDestinationReached(destination dest)
{
if (dest.type != DestType.Explore)
{
return;
}
ExploreCell dest_cell = dest.user_data as ExploreCell;
//Trace.WriteLine($"marking explore cell id {dest_cell?.GlobalId ?? -1} as explored");
if (dest_cell != null)
{
Trace.WriteLine($"Explore cell GID {m_DestCell.GlobalId} considered explored because it was reached.");
OnCellExplored(dest_cell);
}
SelectNewDestinationCell(dest_cell);
}
private void UpdateExploration()
{
Vec3 current_pos = Navmesh.Navigator.CurrentPos;
if (!Enabled || current_pos.IsEmpty)
{
return;
}
if (!IsDataAvailable)
{
Navmesh.Log("[Nav] Exploration data unavailable!");
return;
}
if (IsExplored())
{
Navmesh.Log("[Nav] Exploration finished!");
Navmesh.Navigator.SetDestination(Vec3.Empty, DestType.None);
return;
}
using (new ReadLock(DataLock, true))
{
if (Navmesh.Navigator.GetDestinationType() < DestType.Explore)
{
Vec3 dest = GetDestinationCellPosition();
Navmesh.Navigator.SetDestination(dest, DestType.Explore);
Navmesh.Log("[Nav] Explore dest changed.");
}
{
// mark cells as explored when passing by close enough
ExploreCell current_explore_cell = m_ExploreCells.Find(x => !x.Explored && x.Position.Distance2D(current_pos) < Navmesh.Navigator.ExploreCellPrecision);
if (current_explore_cell != null)
{
OnCellExplored(current_explore_cell);
}
}
}
}
protected ExploreCell GetCurrentExploreCell()
{
Vec3 curr_pos = m_Navigator.CurrentPos;
ExploreCell current_explore_cell = m_ExploreCells.FirstOrDefault(x => x.CellsContains2D(curr_pos));
if (current_explore_cell == null)
{
// try explore cells containing current position first
var potential_explore_cells = m_ExploreCells.Where(x => x.Contains2D(curr_pos));
if (potential_explore_cells == null)
{
potential_explore_cells = m_ExploreCells; // find nearest explore cell
}
if (potential_explore_cells != null)
{
if (potential_explore_cells.Count() == 1)
{
current_explore_cell = potential_explore_cells.First();
}
else
{
float min_dist = float.MaxValue;
foreach (var explore_cell in potential_explore_cells)
{
float dist = explore_cell.Cells.Select(x => x.Distance(curr_pos)).Min();
if (dist < min_dist)
{
current_explore_cell = explore_cell;
min_dist = dist;
}
}
}
}
}
return(current_explore_cell);
}
public bool AddNeighbour(ExploreCell explore_cell)
{
if (!Overlaps2D(explore_cell, true))
return false;
foreach (Cell cell in Cells)
{
foreach (Cell other_cell in explore_cell.Cells)
{
if (cell.Equals(other_cell) || cell.Neighbours.Exists(x => x.cell.Equals(other_cell)))
{
Neighbours.Add(new Neighbour(explore_cell, null, MovementFlag.None));
explore_cell.Neighbours.Add(new Neighbour(this, null, MovementFlag.None));
return true;
}
}
}
return false;
}
protected virtual void OnDeserialize(HashSet <Cell> all_cells, Dictionary <int, Cell> id_to_cell, BinaryReader r)
{
using (new WriteLock(DataLock))
using (new WriteLock(InputLock))
{
m_ExploreCells.Clear();
m_Enabled = r.ReadBoolean();
int explore_cells_count = r.ReadInt32();
// pre-allocate explore cells
for (int i = 0; i < explore_cells_count; ++i)
{
ExploreCell explore_cell = new ExploreCell();
explore_cell.GlobalId = r.ReadInt32();
m_ExploreCells.Add(explore_cell);
}
foreach (ExploreCell explore_cell in m_ExploreCells)
{
explore_cell.Deserialize(m_ExploreCells, all_cells, id_to_cell, r);
}
var dest_cell_global_id = r.ReadInt32();
if (dest_cell_global_id >= 0)
{
m_DestCell = m_ExploreCells.FirstOrDefault(x => x.GlobalId == dest_cell_global_id);
}
ExploreCell.LastExploreCellGlobalId = r.ReadInt32();
m_ExploredCellsCount = r.ReadInt32();
m_CellsToExploreCount = r.ReadInt32();
m_HintPos = new Vec3(r);
}
}
public bool AddNeighbour(ExploreCell explore_cell)
{
if (!Overlaps2D(explore_cell, true))
{
return(false);
}
foreach (Cell cell in Cells)
{
foreach (Cell other_cell in explore_cell.Cells)
{
if (cell.Equals(other_cell) || cell.Neighbours.Exists(x => x.cell.Equals(other_cell)))
{
Neighbours.Add(new Neighbour(explore_cell, Vec3.ZERO, MovementFlag.None));
explore_cell.Neighbours.Add(new Neighbour(this, Vec3.ZERO, MovementFlag.None));
return(true);
}
}
}
return(false);
}
protected List <int> GetUnexploredCellsId(ExploreCell origin_cell)
{
using (new ReadLock(DataLock))
{
List <ExploreCell> unexplored_cells = m_ExploreCells.FindAll(x => !x.Explored);
List <ExploreCell> big_unexplored_cells = unexplored_cells.FindAll(x => !x.Small);
List <int> unexplored_cells_id = null;
// explore small cells only when all other cells were explored
if (big_unexplored_cells.Count == 0)
{
unexplored_cells_id = unexplored_cells.Select(x => x.GlobalId).ToList();
}
else
{
unexplored_cells_id = big_unexplored_cells.Select(x => x.GlobalId).ToList();
}
List <int> potential_cells_id = m_ExploreCellsDistancer.GetConnectedTo(origin_cell.GlobalId);
return(potential_cells_id.Intersect(unexplored_cells_id).ToList()); // consider only unexplored
}
}
public virtual bool IsExplored()
{
using (new ReadLock(DataLock))
{
if (!IsDataAvailable)
{
return(false);
}
ExploreCell current_explore_cell = GetCurrentExploreCell();
if (current_explore_cell == null)
{
return(true);
}
if (!current_explore_cell.Explored)
{
return(false);
}
return(GetUnexploredCells(current_explore_cell).Count == 0);
}
}
public static void FindExplorePath2Opt(ExploreCell start_cell, List <ExploreCell> explore_cells, CellsDistancer distances, ref List <Vec3> path)
{
path.Clear();
if (start_cell == null)
{
return;
}
List <int> cells_id_in_start_cell_network = distances.GetConnectedTo(start_cell.GlobalId);
List <ExploreCell> best_tour = explore_cells.FindAll(c => cells_id_in_start_cell_network.Contains(c.GlobalId));
best_tour.RemoveAll(c => c.Explored || c.Neighbours.Count == 0);
if (start_cell.Explored)
{
best_tour.Insert(0, start_cell);
}
if (best_tour.Count == 1)
{
path.Add(best_tour[0].Position);
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;
}
}
//Navmesh.Log("[FindExplorePath 2-Opt] Final solution distance: " + GetTravelDistance(tour, distances));
foreach (ExploreCell cell in best_tour)
{
path.Add(cell.Position);
}
if (start_cell.Explored)
{
path.RemoveAt(0);
}
}
public static void FindExplorePath(ExploreCell start_cell, List <ExploreCell> explore_cells, CellsDistancer distances, ref List <Vec3> path)
{
//based on http://www.theprojectspot.com/tutorial-post/simulated-annealing-algorithm-for-beginners/6
path.Clear();
if (start_cell == null)
{
return;
}
List <int> cells_id_in_start_cell_network = distances.GetConnectedTo(start_cell.GlobalId);
List <ExploreCell> best_solution = explore_cells.FindAll(c => cells_id_in_start_cell_network.Contains(c.Id));
best_solution.RemoveAll(c => (c.Explored || c.Neighbours.Count == 0) && c.GlobalId != start_cell.GlobalId);
best_solution.Remove(start_cell);
best_solution.Insert(0, start_cell);
if (best_solution.Count == 1)
{
path.Add(best_solution[0].Position);
return;
}
float best_energy = GetTravelDistance(best_solution, distances);
List <ExploreCell> current_solution = best_solution;
float current_energy = best_energy;
Random rng = new Random();
double temp = 10000;
double alpha = 0.999;
double epsilon = 0.0001;
int temp_iterations = 2;
// Loop until system has cooled
while (temp > epsilon)
{
for (int iter = 0; iter < temp_iterations; ++iter)
{
// Create new neighbour tour
List <ExploreCell> new_solution = new List <ExploreCell>(current_solution);
// Get a random positions in the tour
int tour_pos_1 = rng.Next(1, new_solution.Count);
int tour_pos_2 = rng.Next(1, new_solution.Count);
// Swap them
ExploreCell t = new_solution[tour_pos_1];
new_solution[tour_pos_1] = new_solution[tour_pos_2];
new_solution[tour_pos_2] = t;
// Get energy of solutions
float new_energy = GetTravelDistance(new_solution, distances);
// Decide if we should accept the neighbour
if (AcceptanceProbability(current_energy, new_energy, temp) >= rng.NextDouble())
{
current_solution = new_solution;
current_energy = new_energy;
}
// Keep track of the best solution found
if (current_energy < best_energy)
{
best_solution = current_solution;
best_energy = current_energy;
}
}
// Cool system
temp *= alpha;
}
//Navmesh.Log("[FindExplorePath] Final solution distance: " + best_energy);
foreach (ExploreCell cell in best_solution)
{
path.Add(cell.Position);
}
if (start_cell.Explored)
{
path.RemoveAt(0);
}
}
internal virtual void Deserialize(List<Cell> all_cells, BinaryReader r)
{
using (new WriteLock(DataLock))
using (new WriteLock(InputLock))
{
m_ExploreCells.Clear();
m_ExploreCellsDistancer.Clear();
m_Enabled = r.ReadBoolean();
int explore_cells_count = r.ReadInt32();
// pre-allocate explore cells
for (int i = 0; i < explore_cells_count; ++i)
{
ExploreCell explore_cell = new ExploreCell();
explore_cell.GlobalId = r.ReadInt32();
m_ExploreCells.Add(explore_cell);
}
m_ExploreCells.Sort(new Cell.CompareByGlobalId());
foreach (ExploreCell explore_cell in m_ExploreCells)
explore_cell.Deserialize(m_ExploreCells, all_cells, r);
ExploreCell.LastExploreCellGlobalId = r.ReadInt32();
m_ExploreCellsDistancer.Deserialize(r);
m_HintPos = new Vec3(r);
}
}
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 Add(ExploreCell explore_cell)
{
using (new WriteLock(DataLock))
{
foreach (ExploreCell e_cell in m_ExploreCells)
{
if (e_cell.AddNeighbour(explore_cell))
m_ExploreCellsDistancer.Connect(e_cell.GlobalId, explore_cell.GlobalId, e_cell.Position.Distance2D(explore_cell.Position));
}
m_ExploreCells.Add(explore_cell);
}
}
protected List<int> GetUnexploredCellsId(ExploreCell origin_cell)
{
using (new ReadLock(DataLock))
{
List<ExploreCell> unexplored_cells = m_ExploreCells.FindAll(x => !x.Explored);
List<ExploreCell> big_unexplored_cells = unexplored_cells.FindAll(x => !x.Small);
List<int> unexplored_cells_id = null;
// explore small cells only when all other cells were explored
if (big_unexplored_cells.Count == 0)
unexplored_cells_id = unexplored_cells.Select(x => x.GlobalId).ToList();
else
unexplored_cells_id = big_unexplored_cells.Select(x => x.GlobalId).ToList();
List<int> potential_cells_id = m_ExploreCellsDistancer.GetConnectedTo(origin_cell.GlobalId);
return potential_cells_id.Intersect(unexplored_cells_id).ToList(); // consider only unexplored
}
}
protected virtual void OnCellExplored(ExploreCell cell)
{
cell.Explored = true; // this is safe as explore cells cannot be added/removed now
Navmesh.Log("[Nav] Explored cell " + cell.GlobalId + " [progress: " + GetExploredPercent() + "%]!");
}
// 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);
}
}
public float ExploreDistance(ExploreCell cell_1, ExploreCell cell_2)
{
using (new ReadLock(DataLock))
return m_ExploreCellsDistancer.GetDistance(cell_1.GlobalId, cell_2.GlobalId);
}