public RoadMap(ITrapezoidalMap trapezoidalMap, HashSet<ITrapezoid> forbiddenSpace)
{
this.trapezoidalMap = trapezoidalMap;
this.forbiddenSpace = forbiddenSpace;
this.junctionByTrapezoid = new Dictionary<ITrapezoid, Junction>();
foreach (var trapezoid in trapezoidalMap.Trapezoids)
{
if (forbiddenSpace.Contains(trapezoid))
continue;
var position = GetCenterPoint(trapezoid);
var junction = new Junction(position);
junctionByTrapezoid.Add(trapezoid, junction);
}
foreach (var vertical in trapezoidalMap.VerticalEdges)
{
if (forbiddenSpace.Contains(vertical.LeftTrapezoid))
{
Debug.Assert(forbiddenSpace.Contains(vertical.RightTrapezoid));
continue;
}
var position = GetCenterPoint(vertical);
var junction = new Junction(position);
var leftJunction = junctionByTrapezoid[vertical.LeftTrapezoid];
Road.Connect(leftJunction, junction);
var rightJunction = junctionByTrapezoid[vertical.RightTrapezoid];
Road.Connect(rightJunction, junction);
}
}
private Road(Junction junction1, Junction junction2)
{
this.junction1 = junction1;
this.junction2 = junction2;
junction1.AddRoad(this);
junction2.AddRoad(this);
}
public Junction GetNextJunction(Junction starting)
{
if (starting == junction1)
return junction2;
if (starting == junction2)
return junction1;
throw new ArgumentException("Starting junction is not one of the two end points.");
}
private IEnumerable<Junction> SearchForPath(Junction start, Junction goal)
{
var previous = new Dictionary<Junction, Junction>();
var queue = new Queue<Junction>();
queue.Enqueue(start);
previous.Add(start, null);
while (queue.Count != 0)
{
var current = queue.Dequeue();
if (current == goal)
{
var path = new Stack<Junction>();
while (current != null)
{
path.Push(current);
current = previous[current];
}
while (path.Count != 0)
yield return path.Pop();
yield break;
// find a way to just yield from bottom
}
foreach (var nextStop in current.Neighbors)
{
var isVisited = previous.ContainsKey(nextStop);
if (isVisited)
continue;
queue.Enqueue(nextStop);
previous.Add(nextStop, current);
}
}
yield break;
}
public static void Connect(Junction junction1, Junction junction2)
{
new Road(junction1, junction2);
}
public static void Connect(Junction junction1, Junction junction2)
{
new Road(junction1, junction2);
}