void ComputeIntersections(WaypointRoute route1, WaypointRoute route2)
{
List <LineSegment> segments1 = route1.ToLineSegments();
List <LineSegment> segments2 = route2.ToLineSegments();
Double THRESHOLD = 1.0;
foreach (LineSegment ls1 in segments1)
{
foreach (LineSegment ls2 in segments2)
{
LocationValue intersect = BehaviorHelper.LineIntersect(ls1.First.Location, ls1.Second.Location, ls2.First.Location, ls2.Second.Location);
if (intersect != null)
{
// if the intersect is really close to one of the route verticies, than don't do anything.
// otherwise, insert the intersect into the routes
if (BehaviorHelper.Distance(intersect, ls1.First.Location) > THRESHOLD && BehaviorHelper.Distance(intersect, ls1.Second.Location) > THRESHOLD)
{
int pos = route1.IndexOf(ls1.Second);
route1.Insert(pos, new Waypoint(String.Format("between-{0}-{1}", ls1.First.Name, ls1.Second.Name), intersect));
}
if (BehaviorHelper.Distance(intersect, ls2.First.Location) > THRESHOLD && BehaviorHelper.Distance(intersect, ls2.Second.Location) > THRESHOLD)
{
int pos = route2.IndexOf(ls2.Second);
route2.Insert(pos, new Waypoint(String.Format("between-{0}-{1}", ls2.First.Name, ls2.Second.Name), intersect));
}
return; // we only allow for one intersection between the two routes.
}
}
}
}
static public LocationGraph GenerateRouteGraph(String startName, LocationValue startLocation, String endName, LocationValue endLocation, List <WaypointRoute> routes)
{
LocationGraph result = new LocationGraph();
double distanceThreshold = 1.0;
//List<String> routeNames = new List<string>(this.Keys);
String name;
for (int i = 0; i < routes.Count; i++)
{
name = routes[i].Name;
WaypointRoute route = routes[i];
LocationGraph.LocationNode lastNode = null;
foreach (Waypoint wp in route)
{
LocationGraph.LocationNode node = new LocationGraph.LocationNode(wp.Name, wp.Location);
result.AddNode(node);
if (lastNode != null)
{
result.BiConnect(lastNode, node);
}
lastNode = node;
}
if (i > 0)
{
WaypointRoute lastRoute = routes[i - 1];
Boolean done = false;
for (int j = 1; j < lastRoute.Count; j++)
{
if (done)
{
break;
}
for (int k = 1; k < route.Count; k++)
{
if (done)
{
break;
}
Waypoint lastRouteP1 = lastRoute[j - 1];
Waypoint lastRouteP2 = lastRoute[j];
Waypoint nextRouteP1 = route[k - 1];
Waypoint nextRouteP2 = route[k];
LocationValue intersect = BehaviorHelper.LineIntersect(lastRouteP1.Location, lastRouteP2.Location,
nextRouteP1.Location, nextRouteP2.Location);
/*if (intersect != null)
* {
* string newName = String.Format("Intersection_{0}_{1}_{2}_{3}", lastRouteP1.Name, lastRouteP2.Name, nextRouteP1.Name, nextRouteP2.Name);
* LocationGraph.LocationNode intersectNode = new LocationGraph.LocationNode(newName, intersect);
* result.AddNode(intersectNode);
* result.BiConnect(intersectNode.Name, lastRouteP1.Name);
* result.BiConnect(intersectNode.Name, lastRouteP2.Name);
* result.BiConnect(intersectNode.Name, nextRouteP1.Name);
* result.BiConnect(intersectNode.Name, nextRouteP2.Name);
* }
* else*/
if (BehaviorHelper.Distance(lastRouteP1.Location, nextRouteP1.Location) < distanceThreshold)
{
result.BiConnect(lastRouteP1.Name, nextRouteP1.Name);
done = true;
}
else if (BehaviorHelper.Distance(lastRouteP1.Location, nextRouteP2.Location) < distanceThreshold)
{
result.BiConnect(lastRouteP1.Name, nextRouteP2.Name);
done = true;
}
else if (BehaviorHelper.Distance(lastRouteP2.Location, nextRouteP1.Location) < distanceThreshold)
{
result.BiConnect(lastRouteP2.Name, nextRouteP1.Name);
done = true;
}
else if (BehaviorHelper.Distance(lastRouteP2.Location, nextRouteP2.Location) < distanceThreshold)
{
result.BiConnect(lastRouteP2.Name, nextRouteP2.Name);
done = true;
}
}
}
}
}
//result.ProximityConnect(10);
//result.IntersectConnect();
LocationNode startNode = new LocationNode(startName, startLocation);
LocationNode endNode = new LocationNode(endName, endLocation);
result.InsertByProximity(startNode);
result.InsertByProximity(endNode);
return(result);
}