private void Update()
{
if (((prevStartPos - StartPoint).sqrMagnitude > 0.1) ||
(prevGoalPos - GoalPoint).sqrMagnitude > 0.1)
{
currentTask?.Stop();
path = null;
Debug.Log("Start founding path)");
System.GC.Collect();
memoryStart = System.GC.GetTotalMemory(true);
sw = new Stopwatch();
sw.Start();
currentTask = PathFinding.GetPathAsync(StartPoint, GoalPoint);
prevStartPos = StartPoint;
prevGoalPos = GoalPoint;
}
if (currentTask != null && currentTask.Status == PathTaskStatus.Completed)
{
path = currentTask.Path;
currentTask = null;
sw.Stop();
System.GC.Collect();
var memoryEnd = System.GC.GetTotalMemory(true);
Debug.Log("Path finded for " + sw.Elapsed + ", RAM used " + (memoryEnd - memoryStart) / 1024 / 1024 + "MB"); //approximate value
}
}
public SinglePathTask GetPathAsync(Vector2 startPosition, Vector2 goalPosition, double accuracy = 1)
{
var thrId = Interlocked.Increment(ref _threadIdGenerator);
_runnedThreads.Add(thrId);
var pathTask = new SinglePathTask(thrId, this);
TaskFactory.Add(() => GetPathInternalTask(startPosition, goalPosition, accuracy, pathTask));
return(pathTask);
}
private void Start()
{
EventManager.OnMapScanned.Add(10000, () =>
{
Debug.Log("Starting founding pathing)");
System.GC.Collect();
var memoryStart = System.GC.GetTotalMemory(true);
Stopwatch sw = new Stopwatch();
sw.Start();
currentTask = PathFinding.GetPathAsync(StartPoint, GoalPoint);
path = currentTask.WaitForResult();
sw.Stop();
System.GC.Collect();
var memoryEnd = System.GC.GetTotalMemory(true);
Debug.Log("Path finded for " + sw.Elapsed + ", RAM used " + (memoryEnd - memoryStart) / 1024 / 1024 + "MB"); //approximate value
});
prevStartPos = StartPoint;
prevGoalPos = GoalPoint;
}
private void GetPathInternalTask(Vector2 startPosition, Vector2 goalPosition, double accuracy, SinglePathTask pathTask)
{
var startCell = Map.Instance.GetCell(startPosition);
var goalCell = Map.Instance.GetCell(goalPosition);
if (startCell == null)
{
Debug.LogWarning("Start position " + startPosition + " not scanned");
pathTask.Fail();
return;
}
if (goalCell == null)
{
Debug.LogWarning("Goal position " + goalPosition + " not scanned");
pathTask.Fail();
return;
}
//if (startCell.Passability == Cell.MIN_PASSABILITY)
//{
// foreach (var item in startCell.Neighbours)
// {
// if (item.Key.Passability > Cell.MIN_PASSABILITY)
// {
// startCell = item.Key;
// break;
// }
// }
// if (startCell == null)
// {
// _runnedThreads.Remove(threadId);
// if (callback != null)
// callback(param, null);
// Debug.Log("start cell in obstacle");
// return;
// }
//}
if (goalCell.Passability == Cell.MIN_PASSABILITY)
{
foreach (var item in goalCell.Neighbours)
{
if (item.Key.Passability > Cell.MIN_PASSABILITY)
{
goalCell = item.Key;
break;
}
}
if (goalCell.Passability == Cell.MIN_PASSABILITY)
{
Debug.LogWarning("goal cell is obstacle. Cell:" + goalCell.Position);
pathTask.Fail();
return;
}
}
Dictionary <Cell, Node> closed = new Dictionary <Cell, Node>();
Dictionary <Cell, Node> open = new Dictionary <Cell, Node>();
List <Node> sortedOpen = new List <Node>();//todo: change to sorted list
try
{
var PutToOpen = new Action <Node>(node =>
{
for (int i = 0; i < open.Count; i++)
{
if (sortedOpen[i].f < node.f)
{
continue;
}
sortedOpen.Insert(i, node);
open.Add(node.Cell, node);
return;
}
sortedOpen.Insert(open.Count, node);
open.Add(node.Cell, node);
});
var PopFromOpen = new Func <Node>(() =>
{
var result = sortedOpen[0];
sortedOpen.RemoveAt(0);
open.Remove(result.Cell);
return(result);
});
var start = new Node(startCell)
{
g = 0d,
h = Utils.GetDistance(goalCell.Position, startCell.Position) * accuracy
};
start.f = start.g + start.h;
PutToOpen(start);
while (open.Count != 0)
{
if (pathTask.Status == PathTaskStatus.Canceled)
{
return;
}
//if (closed.Count % 100 == 0)
// Thread.Sleep(10);
var x = PopFromOpen();
if (x.Cell.Equals(goalCell))
{
pathTask.Complete(ConstructPath(x));
return;
}
closed.Add(x.Cell, x);
foreach (var yy in x.Cell.Neighbours)
{
if (yy.Key.Passability == Cell.MIN_PASSABILITY)
{
continue;
}
//если текущий сосед содержится в списке просмотренных вершин, то пропустить его
if (closed.ContainsKey(yy.Key))
{
continue;
}
bool tentativeIsBetter = true;
//var tentativeGScore = x.g + 1d;//1d-расстояние между х и соседом
var tentativeGScore = x.g + yy.Value / (yy.Key.Passability / Cell.MAX_PASSABILITY_F);
//Получаем y из open
Node y;
if (open.TryGetValue(yy.Key, out y))
{
if (tentativeGScore < y.g)
{
open.Remove(yy.Key);
sortedOpen.Remove(y);
}
else
{
tentativeIsBetter = false;
}
}
else
{
y = new Node(yy.Key);
}
if (tentativeIsBetter)
{
y.Parent = x;
y.g = tentativeGScore;
y.h = Utils.GetDistance(y.Cell.Position, goalCell.Position) * accuracy;
y.f = y.g + y.h;
PutToOpen(y);
}
}
}
Debug.LogWarning("Goal not founded: StartPos: " + startCell.Position + "\tGoalPos: " + goalCell.Position);
pathTask.Fail();
}
catch (Exception ex)
{
pathTask.Fail();
throw new Exception(ex.Message, ex);
}
finally
{
open.Clear();
sortedOpen.Clear();
closed.Clear();
}
}
