/// <summary>
/// De hoofdfunctie van de pathfinding.
/// </summary>
/// <param name="a">Start positie als AstarObject</param>
/// <param name="b">Eind positie als AstarObject</param>
/// <param name="T"> Het type weg waarin hij moet zoeken</param>
/// <returns></returns>
List<Point> FindRoadPath(Road a, Road b, RoadType T)
{
AstarObject[,] Set = new AstarObject[14, 9];
for (int x = 0; x < 14; x++)
{
for (int y = 0; y < 9; y++)
{
Set[x, y] = new AstarObject(x, y, this);
}
}
Heap<AstarObject> OpenSet = new Heap<AstarObject>(14 * 9);
HashSet<AstarObject> ClosedSet = new HashSet<AstarObject>();
AstarObject Start = Set[a.X, a.Y];
AstarObject End = Set[b.X, b.Y];
OpenSet.Add(Start);
while (OpenSet.Count > 0)
{
AstarObject CurrentLocation = OpenSet.RemoveFirst();
ClosedSet.Add(CurrentLocation);
if (CurrentLocation == End)
{
return RetracePath(Start, End);
//Retracepath and stuff.
}
List<AstarObject> Neighbours = GetNeighbours(CurrentLocation, ref Set, NeighbourhoodType.Neumann, MapsizeXR, MapsizeYR);
foreach (AstarObject neighbour in Neighbours)
{
if (neighbour.RType != T || ClosedSet.Contains(neighbour))
{
continue;
}
int newMovementCostToNeighbour = CurrentLocation.gCost + GetDistance(CurrentLocation, neighbour);
if (newMovementCostToNeighbour < neighbour.gCost || !OpenSet.Contains(neighbour))
{
neighbour.gCost = newMovementCostToNeighbour;
neighbour.hCost = GetDistance(neighbour, End);
neighbour.parent = CurrentLocation;
if (!OpenSet.Contains(neighbour))
{
OpenSet.Add(neighbour);
}
else
{
OpenSet.UpdateItem(neighbour);
}
}
}
}
return new List<Point>();
}
/// <summary>
/// Checkt de naastgelegen posities om de afstanden te bepalen.
/// Heeft een neighbourhoodtype nodig. Moore of Neumann neighbourhood.
/// </summary>
List<AstarObject> GetNeighbours(AstarObject A, ref AstarObject[,] Set, NeighbourhoodType NType, int MapsizeX, int MapsizeY)
{
switch (NType)
{
case NeighbourhoodType.Moore:
List<AstarObject> Neighbours = new List<AstarObject>();
for (int x = -1; x <= 1; x++)
{
for (int y = -1; y <= 1; y++)
{
if (x == 0 && y == 0)
continue;
int CheckX = A.x + x;
int CheckY = A.y + y;
if (CheckX >= 0 && CheckX < MapsizeX && CheckY >= 0 && CheckY < MapsizeY)
{
Neighbours.Add(Set[CheckX, CheckY]);
}
}
}
return Neighbours;
case NeighbourhoodType.Neumann:
if (NType == NeighbourhoodType.Neumann)
{
List<AstarObject> Neighbours2 = new List<AstarObject>();
int CheckX = A.x + 1;
int CheckY = A.y;
if (CheckX >= 0 && CheckX < MapsizeX && CheckY >= 0 && CheckY < MapsizeY)
{
Neighbours2.Add(Set[CheckX, CheckY]);
}
CheckX = A.x - 1;
if (CheckX >= 0 && CheckX < MapsizeX && CheckY >= 0 && CheckY < MapsizeY)
{
Neighbours2.Add(Set[CheckX, CheckY]);
}
CheckX = A.x;
CheckY = A.y - 1;
if (CheckX >= 0 && CheckX < MapsizeX && CheckY >= 0 && CheckY < MapsizeY)
{
Neighbours2.Add(Set[CheckX, CheckY]);
}
CheckX = A.x;
CheckY = A.y + 1;
if (CheckX >= 0 && CheckX < MapsizeX && CheckY >= 0 && CheckY < MapsizeY)
{
Neighbours2.Add(Set[CheckX, CheckY]);
}
return Neighbours2;
}
break;
default:
return null;
}
return null;
}
/// <summary>
/// Gaat het gevonden pad na en reversed de lijst.
/// </summary>
List<Point> RetracePath(AstarObject Start, AstarObject End)
{
List<Point> path = new List<Point>();
AstarObject CurrentA = End;
while (CurrentA != Start)
{
path.Add(CurrentA.ToPoint());
CurrentA = CurrentA.parent;
}
path.Reverse();
return path;
}
/// <summary>
/// Om de afstand te bepalen tussen twee AstarObjects (Posities)
/// </summary>
int GetDistance(AstarObject A, AstarObject B)
{
int dstX = Math.Abs(A.x - B.x);
int dstY = Math.Abs(A.y - B.y);
if (dstX > dstY)
return 14 * dstY + 10 * dstX;
return 14 * dstX + 10 * (dstY - dstX);
}