/// <summary>
/// Gets the nearest path graph vertex from the specified position.
/// </summary>
/// <param name="from"></param>
/// <returns></returns>
public PathGraphVertex FindNearestPathGraphVertex(Vector2 from)
{
PathGraphVertex res = null;
PathGraphVertex fallDownResult = null;
float minDistance = float.MaxValue;
foreach (GridField field in fields)
{
foreach (PathGraphVertex vertex in field.PathGraphVertices)
{
if (fallDownResult == null)
{
fallDownResult = vertex;
}
Vector2 way = (from - vertex.Position.PositionInQuarter);
float direction = (way.GetAngle() + 1 * MathHelper.PiOver2) % MathHelper.TwoPi;
Quadrangle pathObj = Quadrangle.CreateBand(vertex.Position.PositionInQuarter, direction, 0.5f, way.Length());
//Quadrangle pathObj = new Quadrangle(vertex.Position.PositionInQuarter, vertex.Position.PositionInQuarter, from, from);
if ((vertex.Position.PositionInQuarter - from).Length() < minDistance && !IsInCollision(pathObj, x => !(x is Human)))
{
res = vertex;
minDistance = (vertex.Position.PositionInQuarter - from).Length();
}
}
}
if (res != null)
{
return(res);
}
return(fallDownResult);
}
/// <summary>
/// Registers a new path graph vertex into the grid.
/// </summary>
/// <param name="vertex">The path graph vertex</param>
public void AddPathGraphVertex(PathGraphVertex vertex)
{
int x = (int)(vertex.Position.PositionInQuarter.X / fieldWidth);
int y = (int)(vertex.Position.PositionInQuarter.Y / fieldHeight);
GetField(x, y).AddPathGraphVertex(vertex);
}
/// <summary>
/// Searches for shortest path in the graph. Uses Dijkstra 1-1 form (A*).
/// </summary>
/// <param name="from">Source vertex</param>
/// <param name="to">Target vertex</param>
/// <param name="keepInTheSameQuarter">Indicator whether the searching has to use only vertices from the same quarter as the from position is in</param>
/// <returns>Set of vertices forming result path</returns>
public static IEnumerable<PathGraphVertex> FindShortestPath(PathGraphVertex from, PathGraphVertex to, bool keepInTheSameQuarter)
{
keepInTheSameQuarter = keepInTheSameQuarter && from.Position.Quarter == to.Position.Quarter;
TownQuarter fromQuarter = from.Position.Quarter;
LinkedList<PathGraphVertex> resultPath = new LinkedList<PathGraphVertex>();
HashSet<PathGraphVertex> closed = new HashSet<PathGraphVertex>();
Dictionary<PathGraphVertex, PathGraphVertex> cameFrom = new Dictionary<PathGraphVertex, PathGraphVertex>();
Dictionary<PathGraphVertex, float> gScore = new Dictionary<PathGraphVertex, float>();
gScore.Add(from, 0);
Dictionary<PathGraphVertex, float> fScore = new Dictionary<PathGraphVertex, float>();
fScore.Add(from, gScore[from] + HeuristicDistance(from, to));
HashSet<PathGraphVertex> open = new HashSet<PathGraphVertex>();
open.Add( from);
while (open.Count != 0)
{
float lowestScore = float.MaxValue;
PathGraphVertex lowestVertex = null;
foreach (PathGraphVertex openedOne in open)
{
float score = fScore[openedOne];
if (score < lowestScore)
{
lowestVertex = openedOne;
}
}
PathGraphVertex current = lowestVertex;//open.OrderBy(x => fScore[x]).First();
if (current == to)
{
PathGraphVertex t = to;
while(t != from)
{
resultPath.AddFirst(t);
t = cameFrom[t];
}
resultPath.AddFirst(from);
return resultPath;
}
open.Remove(current);
closed.Add(current);
foreach (PathGraphVertex n in current.Neighbors)
{
if (closed.Contains(n) || (keepInTheSameQuarter && n.Position.Quarter != fromQuarter))
{
continue;
}
else
{
float tempGSore = gScore[current] + current.GetDistanceToNeighbor(n);
if (!open.Contains(n) || tempGSore <= gScore[n])
{
cameFrom.SetValue(n, current);
gScore.SetValue(n, tempGSore);
fScore.SetValue(n, gScore[n] + HeuristicDistance(current, n));
if (!open.Contains(n))
{
open.Add(n);
}
}
}
}
}
throw new PathNotFoundException("Source and target vertices aren't in the same component.");
}
static float HeuristicDistance(PathGraphVertex u, PathGraphVertex v)
{
return u.Position.MinimalDistanceTo(v.Position);
}
/// <summary>
/// Generates the path graph part situated in this quarter.
/// </summary>
/// <param name="emptyRectanglesInsideRoads">The inner empty rectangles - the path are around them</param>
/// <returns>List of path graph vertices</returns>
IList<PathGraphVertex> GeneratePathGraph(List<Rectangle> emptyRectanglesInsideRoads)
{
List<Point> pointsOfInterests = new List<Point>();
foreach (Rectangle rect in emptyRectanglesInsideRoads)
{
pointsOfInterests.AddRange( new Point[] {
new Point(rect.X, rect.Y),
//new Point(rect.X + rect.Width - 1, rect.Y),
//new Point(rect.X, rect.Y + rect.Height - 1),
new Point(rect.X + rect.Width - 1, rect.Y + rect.Height - 1)
});
}
Dictionary<Point, Tuple<TownQuarterInterface, bool>> interfaceByPoint = new Dictionary<Point, Tuple<TownQuarterInterface, bool>>();//tuple: interface, isLeftPoint
foreach (var iface in interfaces)
{
Point left, right;
switch (iface.SidePosition)
{
case TownQuarterInterfacePosition.Top:
left.X = iface.BitmapPosition - 1;
left.Y = 0;
right.X = iface.BitmapPosition + 1;
right.Y = 0;
break;
case TownQuarterInterfacePosition.Right:
left.X = bitmapSize.Width - 1;
left.Y = iface.BitmapPosition - 1;
right.X = bitmapSize.Width - 1;
right.Y = iface.BitmapPosition + 1;
break;
case TownQuarterInterfacePosition.Bottom:
left.X = iface.BitmapPosition + 1;
left.Y = bitmapSize.Height - 1;
right.X = iface.BitmapPosition - 1;
right.Y = bitmapSize.Height - 1;
break;
case TownQuarterInterfacePosition.Left:
left.X = 0;
left.Y = iface.BitmapPosition + 1;
right.X = 0;
right.Y = iface.BitmapPosition - 1;
break;
default:
throw new InvalidOperationException("Unknown SidePosition value.");
}
pointsOfInterests.Add(left);
pointsOfInterests.Add(right);
interfaceByPoint.Add(left, new Tuple<TownQuarterInterface, bool>(iface, true));
interfaceByPoint.Add(right, new Tuple<TownQuarterInterface, bool>(iface, false));
}
pointsOfInterests.AddRange(new Point[] {
new Point(BlockWidth - 1, BlockWidth - 1),
new Point(bitmapSize.Width - BlockWidth, bitmapSize.Height - BlockWidth)
});
SortedSet<int> xCoordinates = new SortedSet<int>();
SortedSet<int> yCoordinates = new SortedSet<int>();
foreach (Point point in pointsOfInterests)
{
if (!xCoordinates.Contains(point.X))
xCoordinates.Add(point.X);
if (!yCoordinates.Contains(point.Y))
yCoordinates.Add(point.Y);
}
List<PathGraphVertex> innerVertices = new List<PathGraphVertex>();
Dictionary<int, SortedDictionary<int, PathGraphVertex>> verticalIndexedPaths = new Dictionary<int, SortedDictionary<int, PathGraphVertex>>(yCoordinates.Count);
Dictionary<int, SortedDictionary<int, PathGraphVertex>> horizontalIndexedPaths = new Dictionary<int, SortedDictionary<int, PathGraphVertex>>(xCoordinates.Count);
foreach (int y in yCoordinates)
{
verticalIndexedPaths.Add(y, new SortedDictionary<int, PathGraphVertex>());
}
foreach (int x in xCoordinates)
{
horizontalIndexedPaths.Add(x, new SortedDictionary<int, PathGraphVertex>());
foreach (int y in yCoordinates)
{
if (mapBitmap.Index2D(bitmapSize.Height, x, y) == MapFillType.Sidewalk)
{
PathGraphVertex vertex = new PathGraphVertex(new PositionInTown(this, new Vector2(x + 0.5f, y + 0.5f) * SquareWidth));
innerVertices.Add(vertex);
verticalIndexedPaths[y].Add(x, vertex);
horizontalIndexedPaths[x].Add(y, vertex);
Point p = new Point(x, y);
if (interfaceByPoint.ContainsKey(p))
{
if (interfaceByPoint[p].Item2)
{
interfaceByPoint[p].Item1.LeftPathGraphVertex = vertex;
}
else
{
interfaceByPoint[p].Item1.RightPathGraphVertex = vertex;
}
}
}
}
}
SweepPathVertices(verticalIndexedPaths, AxisDirection.Horizontal);
SweepPathVertices(horizontalIndexedPaths, AxisDirection.Vertical);
return innerVertices;
}
/// <summary>
/// Registers a path graph vertex.
/// </summary>
/// <param name="vertex">The path graph vertex</param>
public void AddPathGraphVertex(PathGraphVertex vertex)
{
pathGraphVertices.Add(vertex);
}
/// <summary>
/// Registers a path graph vertex.
/// </summary>
/// <param name="vertex">The path graph vertex</param>
public void AddPathGraphVertex(PathGraphVertex vertex)
{
pathGraphVertices.Add(vertex);
}
