private void SelectVertex(Vertex2D vertex2D)
{
_wayPoints.Add(vertex2D.Index);
if (vertex2D.Parent != null)
{
SelectVertex(vertex2D.Parent);
}
}
public List <IntVector2> GetPath(IntVector2 point, IntVector2 point2, int pathLength)
{
int localPathLength = 0;
_occupiedIndexes = new List <IntVector2>();
_destinationPoint = point2;
_wayPoints = new List <IntVector2>();
_openListF = new List <int>();
if (IsInOccupiedIndexses(_destinationPoint))
{
return(_wayPoints);
}
Vertex2D first = CreateVertex2D(point, null);
AddInOpenList(first);
while (localPathLength <= 1)
{
_currentVertex = GetMinorVertexByF();
RemoveFromOpenList(_currentVertex);
_closeList.Add(_currentVertex.Index);
List <Vertex2D> neighbours = GetNeibhours(_currentVertex);
foreach (var neighbour in neighbours)
{
AddInOpenList(neighbour);
}
localPathLength++;
}
foreach (var vertex2D in _openList)
{
if (vertex2D.Index.x == _destinationPoint.x && vertex2D.Index.y == _destinationPoint.y)
{
SelectVertex(vertex2D);
}
}
_wayPoints.Reverse();
_openList = new HashSet <Vertex2D>();
_closeList = new HashSet <IntVector2>();
_destinationPoint = new IntVector2(0, 0);
_complete = false;
if (_wayPoints.Count > 0)
{
_wayPoints.RemoveAt(0);
}
return(_wayPoints);
}
private List <Vertex2D> GetNeibhours(Vertex2D vertex)
{
List <Vertex2D> neighbours = new List <Vertex2D>();
Vertex2D neighbour;
neighbour = CreateVertex2D(new IntVector2(vertex.Index.x + 1, vertex.Index.y), vertex);
if (neighbour != null)
{
neighbours.Add(neighbour);
}
neighbour = CreateVertex2D(new IntVector2(vertex.Index.x + 1, vertex.Index.y + 1), vertex);
if (neighbour != null)
{
neighbours.Add(neighbour);
}
neighbour = CreateVertex2D(new IntVector2(vertex.Index.x, vertex.Index.y + 1), vertex);
if (neighbour != null)
{
neighbours.Add(neighbour);
}
neighbour = CreateVertex2D(new IntVector2(vertex.Index.x + 1, vertex.Index.y - 1), vertex);
if (neighbour != null)
{
neighbours.Add(neighbour);
}
neighbour = CreateVertex2D(new IntVector2(vertex.Index.x, vertex.Index.y - 1), vertex);
if (neighbour != null)
{
neighbours.Add(neighbour);
}
neighbour = CreateVertex2D(new IntVector2(vertex.Index.x - 1, vertex.Index.y - 1), vertex);
if (neighbour != null)
{
neighbours.Add(neighbour);
}
neighbour = CreateVertex2D(new IntVector2(vertex.Index.x - 1, vertex.Index.y), vertex);
if (neighbour != null)
{
neighbours.Add(neighbour);
}
neighbour = CreateVertex2D(new IntVector2(vertex.Index.x - 1, vertex.Index.y + 1), vertex);
if (neighbour != null)
{
neighbours.Add(neighbour);
}
return(neighbours);
}
private Vertex2D CreateVertex2D(IntVector2 index, Vertex2D parent)
{
Vertex2D vertex2D = null;
if (!IsPointBusy(index) && !IsInCloseList(index) && !IsInOccupiedIndexses(index))
{
if (_notUsedVevrtexes.Count > 0)
{
vertex2D = _notUsedVevrtexes.Dequeue();
}
else
{
vertex2D = new Vertex2D();
}
vertex2D.Initialize(index, parent, _destinationPoint);
}
return(vertex2D);
}
private void AddInOpenList(Vertex2D vertex)
{
if (vertex.Index.x == _destinationPoint.x && vertex.Index.y == _destinationPoint.y)
{
_notUsedVevrtexes.Enqueue(vertex);
_complete = true;
}
foreach (var vertex2D in _openList)
{
if (vertex2D.Index.x == vertex.Index.x && vertex2D.Index.y == vertex.Index.y)
{
_notUsedVevrtexes.Enqueue(vertex);
return;
}
}
_openListF.Add(vertex.F);
_openList.Add(vertex);
}
public void Initialize(IntVector2 index, Vertex2D parent, IntVector2 destinationPoint)
{
_index = index;
_parent = parent;
if (_parent != null)
{
if (Index.x != parent.Index.x && Index.y != parent.Index.y)
{
_g = parent.G + 14;
}
else
{
_g = parent.G + 10;
}
_h = 10 * (Math.Abs(_index.x - destinationPoint.x) + Math.Abs(_index.y - destinationPoint.y));
}
else
{
_g = _h = 0;
}
}
public List <IntVector2> GetPath(IntVector2 point, IntVector2 point2, List <IntVector2> occupiedIndexes = null)
{
InitializeOccupiedIndexes(occupiedIndexes);
_destinationPoint = point2;
_wayPoints = new List <IntVector2>();
_openListF = new List <int>();
if (IsInOccupiedIndexses(_destinationPoint))
{
DestinationPointIsNotEmpty?.Invoke(_destinationPoint);
return(_wayPoints);
}
if (IsPointBusy(_destinationPoint))
{
DestinationPointIsNotEmpty?.Invoke(_destinationPoint);
return(_wayPoints);
}
Vertex2D first = CreateVertex2D(point, null);
if (first == null)
{
Console.WriteLine("PathFinderController: first point in path is not empty");
}
AddInOpenList(first);
while (!_complete)
{
_currentVertex = GetMinorVertexByF();
RemoveFromOpenList(_currentVertex);
_closeList.Add(_currentVertex.Index);
List <Vertex2D> neighbours = GetNeibhours(_currentVertex);
foreach (var neighbour in neighbours)
{
AddInOpenList(neighbour);
}
if (_openList.Count == 0)
{
_complete = true;
}
}
foreach (var vertex2D in _openList)
{
if (vertex2D.Index.x == _destinationPoint.x && vertex2D.Index.y == _destinationPoint.y)
{
SelectVertex(vertex2D);
}
}
if (_wayPoints.Count == 0)
{
NoWayToDestinationPoint?.Invoke(_destinationPoint);
Reset();
return(_wayPoints);
}
Reset();
_wayPoints.Reverse();
_wayPoints.RemoveAt(0);
return(_wayPoints);
}
private void RemoveFromOpenList(Vertex2D vertex)
{
_openList.Remove(vertex);
_openListF.Remove(vertex.F);
}
