void DebugDrawGraphGizmo(RoadGraph graph, Color edgeColor, Color nodeColor)
{
if (graph == null || graph.nodes == null)
{
return;
}
// Create a hash table of the nodes for fast lookup
var nodes = new Dictionary <int, RoadGraphNode>();
foreach (var node in graph.nodes)
{
nodes.Add(node.nodeId, node);
}
// Draw the edges
Gizmos.color = edgeColor;
foreach (var node in graph.nodes)
{
foreach (var edge in node.adjacentEdges)
{
var otherNode = nodes[edge.otherNodeId];
Gizmos.DrawLine(node.position, otherNode.position);
}
}
// Draw the nodes
Gizmos.color = nodeColor;
foreach (var node in graph.nodes)
{
Gizmos.DrawSphere(node.position, .25f);
}
}
private void LocateAgent()
{
for (int i = 0; i < number_of_carriges; i++)
{
currentLength[i] = -i * (Size.X + 2);
}
if (WayPointsList.Count > 1)
{
CheckPointsList = new List <WayPoint>();
for (int i = 1; i < WayPointsList.Count; i++)
{
RoadGraph.ViewedNodes.Clear();
foreach (var node in RoadGraph.GetNodesWay(WayPointsList[i - 1], WayPointsList[i]))
{
if (CheckPointsList.Count == 0 || !CheckPointsList[CheckPointsList.Count - 1].Equals(node))
{
CheckPointsList.Add(node);
}
}
}
positions[0] = CheckPointsList[0].LocationPoint;
position = CheckPointsList[0].LocationPoint;
need_draw[0] = true;
CurrentSpeed = MaxSpeed;
}
}
/// <summary>
/// Build graph from OSM elements. May contains duplicated elements (from multiple calls)
/// </summary>
/// <param name="elements"></param>
/// <returns></returns>
private static RoadGraph BuildGraph(List <Element> elements, bool ignoreOneWays)
{
RoadGraph result = new RoadGraph();
IEnumerable <Way> ways = elements.Where(x => x is Way).Select(x => x as Way).Distinct();
var nodeLookup = new Dictionary <long, OsmNode>();
foreach (var node in elements.Where(x => x is OsmNode).Select(x => x as OsmNode))
{
nodeLookup[node.Id] = node;
}
var intersectionCounter = ways.Select(x => x.Nodes).CountRepeatedIds();
foreach (var way in ways)
{
string wayName = way.ParseName();
var subways = way.Nodes.InclusivePartitioning(id => intersectionCounter[id] > 1);
foreach (var subway in subways)
{
// TODO : why are ids not in dictinoary sometimes? Data problem?
var roadShape = subway.Where(id => nodeLookup.ContainsKey(id)).Select(id => nodeLookup[id].ToCoord()).ToList();
DirectedRoad road = new DirectedRoad(subway.First().ToString(), subway.Last().ToString(), roadShape, wayName);
result.AddRoad(road);
if (!way.IsOneWay() || ignoreOneWays)
{
DirectedRoad reverseRoad = road.Reverse();
result.AddRoad(reverseRoad);
}
}
}
return(result);
}
public MapMatcher(List <T> data, Func <T, DirectedRoad> dataToRoad, MapMatcherParameters parameters, bool useSearchGrid = true, BoundingBox boundingBox = null)
{
// Initialize parameters
Parameters = parameters;
//Build graph
_dataByRoadId = new Dictionary <string, T>();
var graph = new RoadGraph();
foreach (var datum in data)
{
var road = dataToRoad(datum);
graph.AddRoad(road);
_dataByRoadId[road.Squid] = datum;
}
Graph = graph;
// Compute search grid (for accessing nearby roads)
if (useSearchGrid)
{
// Compute bounding box if not given
if (boundingBox == null)
{
boundingBox = graph.Nodes.Values.GetBoundingBox();
}
SearchGrid = SearchGridFactory.ComputeSearchGrid(graph, parameters.NearbyRoadsThreshold, boundingBox);
}
// Initialize state
State = MapMatchState.InitialState();
}
public HashSet <HotRoute> Run(RoadGraph roadGraph, int eps, int minTraffic)
{
this.eps = eps;
this.minTraffic = minTraffic;
this.roadGraph = roadGraph;
hotRoutes = new HashSet <HotRoute>();
hotRouteStarts = new HashSet <Connection>();
//var test = roadGraph.Connections.Where(c => c.Traffic.Count() >= minTraffic);
//Console.WriteLine(test.ToList().Count);
this.aStart = new AstarPathfinder(this.roadGraph);
FindHotRouteStarts();
Console.WriteLine("Return FindHotRouteStars(): " + hotRouteStarts.Count);
foreach (var hrs in hotRouteStarts)
{
var r = new HotRoute(hrs);
ExtendHotRoutes(ref r);
hotRoutes.Add(r);
}
return(hotRoutes);
}
//public static void PlotGraph(RoadGraph graph, GMapOverlay overlay, Color color)
//{
// foreach (var intersection in graph.Values)
// {
// //PlotPoint(nodeCoord.Latitude, nodeCoord.Longitude, GMarkerGoogleType.blue_dot,overlay);
// foreach (var link in intersection.Links)
// {
// PlotPolyLine(link, overlay, color, 3, true);
// }
// }
//}
public static void PlotGraph(RoadGraph graph, GMapOverlay overlay, Color color)
{
foreach (var road in graph.Roads)
{
PlotRoute(road.Geometry, overlay, color);
}
}
public static RoadSearchGrid ComputeSearchGrid(RoadGraph graph, double gridSizeInMeters, BoundingBox boundingBox = null)
{
var stopwatch = new System.Diagnostics.Stopwatch();
stopwatch.Start();
double minLat = double.NaN;
double maxLat = double.NaN;
double minLng = double.NaN;
double maxLng = double.NaN;
if (boundingBox != null)
{
minLat = boundingBox.LatLowerBound;
maxLat = boundingBox.LatUpperBound;
minLng = boundingBox.LngLowerBound;
maxLng = boundingBox.LngUpperBound;
}
else
{
List <Coord> allCoords = graph.Roads.SelectMany(x => x.Geometry).ToList();
var latitudes = allCoords.Select(x => x.Latitude);
var longitudes = allCoords.Select(x => x.Longitude);
minLat = latitudes.Min();
maxLat = latitudes.Max();
minLng = longitudes.Min();
maxLng = longitudes.Max();
}
// Get rough lat/lng delta values corresponding to the grid size
double refLatInRadians = (((minLat + maxLat) / 2) * (Math.PI / 180.0));
double roughLngGridSize = ((gridSizeInMeters / (Constants.Earth_Radius_In_Meters * Math.Cos(refLatInRadians))) * (180.0 / Math.PI));
double roughLatGridSize = ((gridSizeInMeters / Constants.Earth_Radius_In_Meters) * (180.0 / Math.PI));
// Get total width and height of the grid
double lngWidth = (maxLng + roughLngGridSize) - (minLng - roughLngGridSize);
double latHeight = (maxLat + roughLatGridSize) - (minLat - roughLatGridSize);
// Corners are the minimum values with an added margin
double left = minLng - roughLngGridSize;
double bottom = minLat - roughLatGridSize;
// Dimensions are the total length over the grid size ( may not divide in perfectly)
int cellCountX = (int)(lngWidth / roughLngGridSize);
int cellCountY = (int)(latHeight / roughLatGridSize);
// Actual grid sizes are obtained from the above dimensions and the length the sides
double gridSizeX = lngWidth / cellCountX;
double gridSizeY = latHeight / cellCountY;
var result = new RoadSearchGrid(left, bottom, gridSizeX, gridSizeY, cellCountX, cellCountY);
// TODO: correct roads input below??
result.Populate(graph.Values.SelectMany(x => x));
stopwatch.Stop();
return(result);
}
private void LocateAgent()
{
if (WayPointsList.Count > 1)
{
RoadGraph.ViewedNodes.Clear();
CheckPointsList = RoadGraph.GetNodesWay(WayPointsList[0], WayPointsList[WayPointsList.Count - 1]);
position = CheckPointsList[0].LocationPoint;
CurrentSpeed = MaxSpeed;
}
}
public static Transition TransitionProbability(RoadGraph graph, ProjectToRoad projection1, ProjectToRoad projection2, MapMatcherParameters parameters)
{
DirectedRoad road1 = projection1.Road;
DirectedRoad road2 = projection2.Road;
//calculate on road distance
double onRoadDistanceInMeters;
Distance startingDist = projection1.DistanceToEnd;
Distance endDist = projection2.DistanceFromStart;
// Roads are the same:
if (road1.Equals(road2))
{
//negative if going backwards along road
onRoadDistanceInMeters = projection2.DistanceFromStart.DistanceInMeters - projection1.DistanceFromStart.DistanceInMeters;
}
// Road start or end on the same node
else if (road1.End == road2.End || road1.Start == road2.Start)
{
//make this transition impossible
return(Transition.ImpossibleTransition(road1, road2));
}
// Roads are connected (can be same road in opposite direction)
else if (road1.End == road2.Start)
{
onRoadDistanceInMeters = startingDist.DistanceInMeters + endDist.DistanceInMeters;
}
// Try connect roads using Dijstra
else
{
List <DirectedRoad> path;
if (PathFinding.DijstraTryFindPath(graph, road1.End, road2.Start, parameters.DijstraUpperSearchLimit, out path))
{
Distance connectingDist = Distance.Zero;
foreach (var road in path)
{
connectingDist += road.Length;
}
onRoadDistanceInMeters = startingDist.DistanceInMeters + connectingDist.DistanceInMeters + endDist.DistanceInMeters;
}
else
{
//cannot connect up roads. transition probability is zero
return(Transition.ImpossibleTransition(road1, road2));
}
}
Distance haversineDistance = projection1.Coordinate.HaversineDistance(projection2.Coordinate);
return(new Transition(onRoadDistanceInMeters, haversineDistance.DistanceInMeters, road1, road2, parameters));
}
private void Button_Click(object sender, RoutedEventArgs e)
{
bool has_dont_linked = false;
for (int i = 0; i < VertexVisualsList.Count; i++)
{
if (VertexVisualsList[i].InCount == 0 && VertexVisualsList[i].OutCount == 0)
{
has_dont_linked = true;
}
}
if (has_dont_linked)
{
if (MessageBox.Show("Все точки, не имеющие связей (обозначены серым цветом) будут удалены. Продолжить?", "Внимание", MessageBoxButton.YesNo) == MessageBoxResult.No)
{
return;
}
List <WayPoint> remList = new List <WayPoint>();
foreach (var node in RoadGraph.Nodes)
{
int in_count = 0, out_count = 0;
foreach (var edge in RoadGraph.GetEdgesTo(node))
{
in_count++;
}
foreach (var edge in RoadGraph.GetEdgesFrom(node))
{
out_count++;
}
if (in_count == 0 && out_count == 0)
{
remList.Add(node);
}
}
for (int i = 0; i < remList.Count; i++)
{
RoadGraph.Remove(remList[i]);
}
}
//Собираем граф дорожной сети
//RoadGraph = new Graph<WayPoint, PathFigure>();
for (int i = 0; i < VertexVisualsList.Count; i++)
{
RoadGraph.Add(VertexVisualsList[i].Node);
}
for (int i = 0; i < EdgeVisualsList.Count; i++)
{
RoadGraph.AddEdge(EdgeVisualsList[i].NodeFrom, EdgeVisualsList[i].NodeTo, EdgeVisualsList[i].PathData);
}
DialogResult = true;
Close();
}
public OsmMapMatcher(RoadGraph graph, MapMatcherParameters parameters = null, bool useSearchGrid = true)
{
if (parameters == null)
{
parameters = MapMatcherParameters.Default;
}
Parameters = parameters;
if (useSearchGrid)
{
SearchGrid = SearchGridFactory.ComputeSearchGrid(graph, DefaultValues.Nearby_Road_Radius_In_Meters);
}
Graph = graph;
State = MapMatchState.InitialState();
}
private void Awake()
{
if (Instance == null)
{
Instance = this;
}
if (Instance != this)
{
Destroy(gameObject);
}
roadGraph = new RoadGraph();
roadGraph.Init();
}
private void LocateAgent()
{
if (RouteList.Count > 1 || RoadGraph == null)
{
RoadGraph.ViewedNodes.Clear();
RouteList = RoadGraph.GetNodesWay(RouteList.First(), RouteList.Last());
Position = RouteList.First().Location;
base.CurrentSpeed = base._maxSpeed;
_located = true;
}
else
{
RouteList.Clear();
}
}
public static RoadGraph BuildGraph1()
{
RoadGraph result = new RoadGraph();
DirectedRoad roadAB = new DirectedRoad("1000", "1001", GeometryAB().ToCoordList(), "AB");
DirectedRoad roadAC = new DirectedRoad("1000", "1002", GeometryAC().ToCoordList(), "AC");
DirectedRoad roadAD = new DirectedRoad("1000", "1003", GeometryAD().ToCoordList(), "AD");
DirectedRoad roadDB = new DirectedRoad("1003", "1001", GeometryDB().ToCoordList(), "DB");
result.AddRoad(roadAB);
result.AddRoad(roadAC);
result.AddRoad(roadAD);
result.AddRoad(roadDB);
return result;
}
////////////////
// Graph where A->B, B->C is shorter than going A->C
public static RoadGraph BuildGraphTwoShortRoadsOneLong()
{
RoadGraph result = new RoadGraph();
// Two short roads
DirectedRoad roadAB = new DirectedRoad("1000", "1001", StraightAB().ToCoordList(), "AB");
DirectedRoad roadBC = new DirectedRoad("1001", "1002", StraightBC().ToCoordList(), "BC");
//Long road
DirectedRoad roadAC = new DirectedRoad("1000", "1002", BigTriangleAC().ToCoordList(), "AC");
result.AddRoad(roadAB);
result.AddRoad(roadBC);
result.AddRoad(roadAC);
return result;
}
public MapInfo(int mapId, int width, int height, MapSettings settings, int numberOfPlayers, CancellationToken cancellationToken)
{
token = cancellationToken;
MapId = mapId;
Settings = settings;
ObjectFactory = new ObjectFactory();
Tiles = new Grid <TileInfo>(width, height);
TextureData = new Grid <Texture>(width + 1, height + 1);
StartingPositions = new Coordinates[numberOfPlayers];
Roads = new RoadGraph();
FlattenedAreas = new List <Circle>();
BaseRadius = 40;
ResourceRadiusNear = 5;
ResourceRadiusFar = 40;
MapIniTemplates = new List <string>();
}
private Point CalculateCoordinateLocationOfCarrige()
{
double tmp = currentLength;
for (int i = 1; i < CheckPointsList.Count; i++)
{
//Поиск дороги
PathFigure road = null;
var edges = RoadGraph.GetEdgesFrom(CheckPointsList[i - 1]).GetEnumerator();
while (edges.MoveNext())
{
if (edges.Current.End.Equals(CheckPointsList[i]))
{
road = edges.Current.Data;
break;
}
}
//Если дорога не найдена
if (road == null)
{
CalculateAngle(position, CheckPointsList[i].LocationPoint);
position = CheckPointsList[i].LocationPoint;
return(new Point(-1, -1));
}
//Если найдена
double road_lenght = CalculateLength(road);
//Если этот участок уже пройден, то смотрим следующий
if (tmp > road_lenght)
{
tmp -= road_lenght;
continue;
}
for (int g = 0; g < road.Segments.Count; g++)
{
double x = 0, y = 0, t = 0, seg_l = 0;
Point startPoint = road.StartPoint;
if (road.Segments[g] is BezierSegment)
{
BezierSegment seg = road.Segments[g] as BezierSegment;
seg_l = CalculateLength(new PathFigure(startPoint, new List <PathSegment>()
{
seg
}, false));
if (seg_l < tmp)
{
tmp -= seg_l;
startPoint = seg.Point3;
continue;
}
t = tmp / seg_l;
x = (1 - t) * (1 - t) * (1 - t) * startPoint.X + (1 - t) * (1 - t) * 3 * t * seg.Point1.X + (1 - t) * 3 * t * t * seg.Point2.X + t * t * t * seg.Point3.X;
y = (1 - t) * (1 - t) * (1 - t) * startPoint.Y + (1 - t) * (1 - t) * 3 * t * seg.Point1.Y + (1 - t) * 3 * t * t * seg.Point2.Y + t * t * t * seg.Point3.Y;
}
else if (road.Segments[g] is LineSegment)
{
LineSegment seg = road.Segments[g] as LineSegment;
seg_l = CalculateLength(new PathFigure(startPoint, new List <PathSegment>()
{
seg
}, false));
if (seg_l < tmp)
{
tmp -= seg_l;
startPoint = seg.Point;
continue;
}
t = tmp / seg_l;
x = seg.Point.X * t + startPoint.X * (1 - t);
if (startPoint.X == seg.Point.X)
{
y = startPoint.Y + (seg.Point.Y - startPoint.Y) * t;
}
else
{
y = (x - startPoint.X) * (seg.Point.Y - startPoint.Y) / (seg.Point.X - startPoint.X) + startPoint.Y;
}
}
else if (road.Segments[g] is PolyLineSegment)
{
PolyLineSegment seg = road.Segments[g] as PolyLineSegment;
int l;
for (l = 0; l < seg.Points.Count; l++)
{
seg_l = Math.Sqrt(Math.Pow(startPoint.X - seg.Points[l].X, 2) + Math.Pow(startPoint.Y - seg.Points[l].Y, 2));
if (seg_l < tmp)
{
tmp -= seg_l;
startPoint = seg.Points[l];
continue;
}
}
t = tmp / seg_l;
x = seg.Points[l].X * t + startPoint.X * (1 - t);
y = (x - startPoint.X) * (seg.Points[l].Y - startPoint.Y) / (seg.Points[l].X - startPoint.X) + startPoint.Y;
}
CalculateAngle(position, new Point(x, y));
return(new Point(x, y));
}
}
return(new Point(-1, -1));
}
//Creates the initial Scene point.
public void Start()
{
rand = new System.Random();
graph = new RoadGraph <RoadNode>();
//Initialize the initial state.
//Adjacents.
/*
* graph.add(
* new RoadNode(
* true,
* GameStateController.Instance.GetComponent<BuildingPrefabWrapper>().home,
* randomSingleBuilding(),
* RoadNode.EndType.Four,
* RoadNode.EndType.Four,
* new Vector2(1,0)
* ));
*
* graph.add(
* new RoadNode(
* false,
* graph.GetNode(new Vector2(1,0)).getblockNW(),
* randomSingleBuilding(),
* RoadNode.EndType.Four,
* RoadNode.EndType.Four,
* new Vector2(0, -1)
* ));
*
* graph.add(
* new RoadNode(
* true,
* randomSingleBuilding(),
* graph.GetNode(new Vector2(1,0)).getblockNW(),
* RoadNode.EndType.Four,
* RoadNode.EndType.Four,
* new Vector2(-1, 0)
* ));
*
* graph.add(
* new RoadNode(
* false,
* randomSingleBuilding(),
* graph.GetNode(new Vector2(1,0)).getblockNW(),
* RoadNode.EndType.Four,
* RoadNode.EndType.Four,
* new Vector2(0, 1)
* ));
*
* //Verticals
* graph.add(
* new RoadNode(
* true,
* graph.GetNode(new Vector2(0, 1)).getblockNW(),
* randomSingleBuilding(),
* RoadNode.EndType.Four,
* RoadNode.EndType.Four,
* new Vector2(1, 2)
* ));
*
* graph.add(
* new RoadNode(
* true,
* graph.GetNode(new Vector2(0, -1)).getblockES(),
* randomSingleBuilding(),
* RoadNode.EndType.Four,
* RoadNode.EndType.Four,
* new Vector2(1, -2)
* ));
*
* graph.add(
* new RoadNode(
* true,
* randomSingleBuilding(),
* graph.GetNode(new Vector2(0, -1)).getblockES(),
* RoadNode.EndType.Four,
* RoadNode.EndType.Four,
* new Vector2(-1, -2)
* ));
*
* graph.add(
* new RoadNode(
* true,
* randomSingleBuilding(),
* graph.GetNode(new Vector2(0, 1)).getblockNW(),
* RoadNode.EndType.Four,
* RoadNode.EndType.Four,
* new Vector2(-1, 2)
* ));
*
* //Fill in.
* graph.add(
* new RoadNode(
* false,
* graph.GetNode(new Vector2(1, 2)).getblockES(),
* graph.GetNode(new Vector2(1, 0)).getblockES(),
* RoadNode.EndType.Four,
* RoadNode.EndType.Four,
* new Vector2(2, 1)
* ));
*
* graph.add(
* new RoadNode(
* false,
* graph.GetNode(new Vector2(1, 0)).getblockES(),
* graph.GetNode(new Vector2(1, -2)).getblockES(),
* RoadNode.EndType.Four,
* RoadNode.EndType.Four,
* new Vector2(2, -1)
* ));
*
* graph.add(
* new RoadNode(
* false,
* graph.GetNode(new Vector2(-1, 0)).getblockES(),
* graph.GetNode(new Vector2(-1, -2)).getblockES(),
* RoadNode.EndType.Four,
* RoadNode.EndType.Four,
* new Vector2(-2, -1)
* ));
*
* graph.add(
* new RoadNode(
* false,
* graph.GetNode(new Vector2(-1, 2)).getblockES(),
* graph.GetNode(new Vector2(-1, 0)).getblockES(),
* RoadNode.EndType.Four,
* RoadNode.EndType.Four,
* new Vector2(-2, 1)
* ));
*
* //Edges of 3x3 block.
* graph.add(
* new RoadNode(
* true,
* graph.GetNode(new Vector2(2, 1)).getblockNW(),
* null,
* RoadNode.EndType.Four,
* RoadNode.EndType.Four,
* new Vector2(3, 2)
* ));
*
* graph.add(
* new RoadNode(
* true,
* graph.GetNode(new Vector2(1, 0)).getblockES(),
* null,
* RoadNode.EndType.Four,
* RoadNode.EndType.Four,
* new Vector2(3, 0)
* ));
*
* graph.add(
* new RoadNode(
* true,
* graph.GetNode(new Vector2(2, -1)).getblockES(),
* null,
* RoadNode.EndType.Four,
* RoadNode.EndType.Four,
* new Vector2(3, -2)
* ));
*
* graph.add(
* new RoadNode(
* false,
* graph.GetNode(new Vector2(2, -1)).getblockES(),
* null,
* RoadNode.EndType.Four,
* RoadNode.EndType.Four,
* new Vector2(2, -3)
* ));
*
* graph.add(
* new RoadNode(
* false,
* graph.GetNode(new Vector2(0, -1)).getblockES(),
* null,
* RoadNode.EndType.Four,
* RoadNode.EndType.Four,
* new Vector2(0, -3)
* ));
*
* graph.add(
* new RoadNode(
* false,
* graph.GetNode(new Vector2(-2, -1)).getblockES(),
* null,
* RoadNode.EndType.Four,
* RoadNode.EndType.Four,
* new Vector2(-2, -3)
* ));
*
* graph.add(
* new RoadNode(
* true,
* null,
* graph.GetNode(new Vector2(-1, -2)).getblockNW(),
* RoadNode.EndType.Four,
* RoadNode.EndType.Four,
* new Vector2(-3, -2)
* ));
*
* graph.add(
* new RoadNode(
* true,
* null,
* graph.GetNode(new Vector2(-1, 0)).getblockNW(),
* RoadNode.EndType.Four,
* RoadNode.EndType.Four,
* new Vector2(-3, 0)
* ));
*
* graph.add(
* new RoadNode(
* true,
* null,
* graph.GetNode(new Vector2(-1, 2)).getblockNW(),
* RoadNode.EndType.Four,
* RoadNode.EndType.Four,
* new Vector2(-3, 2)
* ));
*
* graph.add(
* new RoadNode(
* false,
* null,
* graph.GetNode(new Vector2(-2, 1)).getblockNW(),
* RoadNode.EndType.Four,
* RoadNode.EndType.Four,
* new Vector2(-2, 3)
* ));
*
* graph.add(
* new RoadNode(
* false,
* null,
* graph.GetNode(new Vector2(0, 1)).getblockNW(),
* RoadNode.EndType.Four,
* RoadNode.EndType.Four,
* new Vector2(0, 3)
* ));
*
* graph.add(
* new RoadNode(
* false,
* null,
* graph.GetNode(new Vector2(2, 1)).getblockNW(),
* RoadNode.EndType.Four,
* RoadNode.EndType.Four,
* new Vector2(2, 3)
* ));
*
*
* //Edges.
* graph.add(graph.GetNode(new Vector2(0, 1)), graph.GetNode(new Vector2(1, 0)));
* graph.add(graph.GetNode(new Vector2(1, 0)), graph.GetNode(new Vector2(0, 1)));
*
* graph.add(graph.GetNode(new Vector2(1, 0)), graph.GetNode(new Vector2(0, -1)));
* graph.add(graph.GetNode(new Vector2(0, -1)), graph.GetNode(new Vector2(1, 0)));
*
* graph.add(graph.GetNode(new Vector2(0, -1)), graph.GetNode(new Vector2(-1, 0)));
* graph.add(graph.GetNode(new Vector2(-1, 0)), graph.GetNode(new Vector2(0, -1)));
*
* graph.add(graph.GetNode(new Vector2(-1, 0)), graph.GetNode(new Vector2(0, 1)));
* graph.add(graph.GetNode(new Vector2(0, 1)), graph.GetNode(new Vector2(-1, 0)));
*
* //---
*
* graph.add(graph.GetNode(new Vector2(0, 1)), graph.GetNode(new Vector2(1, 2)));
* graph.add(graph.GetNode(new Vector2(1, 2)), graph.GetNode(new Vector2(0, 1)));
*
* graph.add(graph.GetNode(new Vector2(1, 2)), graph.GetNode(new Vector2(2, 1)));
* graph.add(graph.GetNode(new Vector2(2, 1)), graph.GetNode(new Vector2(1, 2)));
*
* graph.add(graph.GetNode(new Vector2(2, 1)), graph.GetNode(new Vector2(1, 0)));
* graph.add(graph.GetNode(new Vector2(1, 0)), graph.GetNode(new Vector2(2, 1)));
*
* //---
*
* graph.add(graph.GetNode(new Vector2(1, 0)), graph.GetNode(new Vector2(2, -1)));
* graph.add(graph.GetNode(new Vector2(2, -1)), graph.GetNode(new Vector2(1, 0)));
*
* graph.add(graph.GetNode(new Vector2(2, -1)), graph.GetNode(new Vector2(1, -2)));
* graph.add(graph.GetNode(new Vector2(1, -2)), graph.GetNode(new Vector2(2, -1)));
*
* graph.add(graph.GetNode(new Vector2(1, -2)), graph.GetNode(new Vector2(0, -1)));
* graph.add(graph.GetNode(new Vector2(0, -1)), graph.GetNode(new Vector2(1, -2)));
*
* //---
*
* graph.add(graph.GetNode(new Vector2(0, -1)), graph.GetNode(new Vector2(-1, -2)));
* graph.add(graph.GetNode(new Vector2(-1, -2)), graph.GetNode(new Vector2(0, -1)));
*
* graph.add(graph.GetNode(new Vector2(-1, -2)), graph.GetNode(new Vector2(-2, -1)));
* graph.add(graph.GetNode(new Vector2(-2, -1)), graph.GetNode(new Vector2(-1, -2)));
*
* graph.add(graph.GetNode(new Vector2(-2, -1)), graph.GetNode(new Vector2(-1, 0)));
* graph.add(graph.GetNode(new Vector2(-1, 0)), graph.GetNode(new Vector2(-2, -1)));
*
* //---
*
* graph.add(graph.GetNode(new Vector2(-1, 0)), graph.GetNode(new Vector2(-2, 1)));
* graph.add(graph.GetNode(new Vector2(-2, 1)), graph.GetNode(new Vector2(-1, 0)));
*
* graph.add(graph.GetNode(new Vector2(-2, 1)), graph.GetNode(new Vector2(-1, 2)));
* graph.add(graph.GetNode(new Vector2(-1, 2)), graph.GetNode(new Vector2(-2, 1)));
*
* graph.add(graph.GetNode(new Vector2(-1, 2)), graph.GetNode(new Vector2(0, 1)));
* graph.add(graph.GetNode(new Vector2(0, 1)), graph.GetNode(new Vector2(-1, 2)));
*/
StartCoroutine(generateBlock(new Vector3(0, 0, 0), Direction.NONE));
}
public override void Step()
{
try
{
if (position == new Point(-1, -1))
{
LocateAgent();
}
//Длина пути, которая будет пройдена за один шаг моделирвания
double step_lenght = (double)Scenario.STEP_TIME_MS / CurrentSpeed;
double tmp = currentLength;
double route_length = 0;
for (int i = 1; i < CheckPointsList.Count; i++)
{
//Поиск дороги
PathFigure road = null;
var edges = RoadGraph.GetEdgesFrom(CheckPointsList[i - 1]).GetEnumerator();
while (edges.MoveNext())
{
if (edges.Current.End.Equals(CheckPointsList[i]))
{
road = edges.Current.Data;
break;
}
}
//Расчет длины маршрута и данного участка
double road_length = CalculateLength(road);
route_length += road_length;
//Если этот участок еще не пройден
if (tmp < road_length)
{
//Если участок не будет пройден за этот шаг
if (tmp + step_lenght + Size.X / 2 < road_length)
{
currentLength += step_lenght;
position = CalculateCoordinateLocationOfCarrige();
break;
}
else if (tmp + step_lenght + Size.X / 2 > road_length)
{
if (tmp + Size.X / 2 <= road_length)
{
if (CheckPointsList[i].IsServicePoint)
{
//Паркуемся
if (tmp != road_length - Size.X / 2)
{
currentLength += road_length - tmp - Size.X / 2;
position = CalculateCoordinateLocationOfCarrige();
}
//Работаем с сервисом
if (!ReadyToIOOperation)
{
ServiceBase service = scenario.ServicesList.Find(delegate(ServiceBase sb) { return(sb.ID == CheckPointsList[i].ServiceID); });
if (service.ID == -1)
{
throw new ArgumentException("Service not found, service id = " + CheckPointsList[i].ServiceID);
}
if (service is StopService)
{
(service as StopService).OpenInputPoints(ID);
ReadyToIOOperation = true;
}
}
if (go)
{
ReadyToIOOperation = false;
go = false;
}
if (ReadyToIOOperation)
{
return;
}
}
//Задержка на маршрутной точке
if (CheckPointsList[i].IsWaitPoint)
{
if (CheckPointsList[i].MinWait > 0)
{
CheckPointsList[i].MinWait -= Scenario.STEP_TIME_MS;
return;
}
}
//Точка с присвоением фиксированной скорости
if (CheckPointsList[i].IsFixSpeedPoint)
{
CurrentSpeed = Convert.ToInt32(CheckPointsList[i].FixSpeed);
}
else
{
CurrentSpeed = MaxSpeed;
}
if (CheckPointsList.Count > i + 1 && i > 0 && CheckPointsList[i + 1].Equals(CheckPointsList[i - 1]))
{
Revers(route_length);
}
}
currentLength += step_lenght;
position = CalculateCoordinateLocationOfCarrige();
break;
}
}
else
{
if (i == CheckPointsList.Count - 1)
{
currentLength += step_lenght;
}
tmp -= road_length;
}
}
}
catch (Exception ex)
{
Console.WriteLine(string.Format("Error in Thread! Agent ID [{0}]. Error message [{1}]", ID, ex.Message));
WayPointsList.Clear();
return;
}
}
/// <summary>
/// Connects gaps and removes duplicates in a sequence of roads in a graph.
/// </summary>
/// <param name="graph"></param>
/// <param name="sequence"></param>
/// <returns></returns>
public static List <DirectedRoad> DijstraConnectUpSequence(this List <DirectedRoad> sequence, RoadGraph graph)
{
if (sequence.Count == 0)
{
throw new System.ArgumentException("Empty sequence");
}
if (sequence.Count == 1)
{
return(sequence);
}
var cleanedSequence = sequence.RemoveDuplicateNeighbors();
var result = new List <DirectedRoad>()
{
cleanedSequence.First()
};
for (int i = 1; i < cleanedSequence.Count; i++)
{
//add all necessary roads up to and including the i'th
List <DirectedRoad> connection;
if (DijstraTryFindPath(graph, cleanedSequence[i - 1].End, cleanedSequence[i].Start, double.PositiveInfinity, out connection))
{
result.AddRange(connection);
result.Add(cleanedSequence[i]);
}
else
{
throw new Exception("Unable to connect up sequence of roads");
}
}
return(result);
}
/// <summary>
/// Finds the shortest sequence of directed roads from the origin to destination node using Dijstra's algorithm.
/// </summary>
/// <param name="graph"></param>
/// <param name="origin"></param>
/// <param name="destination"></param>
/// <returns></returns>
public static bool DijstraTryFindPath(RoadGraph graph, string origin, string destination, double upperSearchLimitInMeters, out List <DirectedRoad> path)
{
path = new List <DirectedRoad>();
// If there are no outgoing roads from the origin
if (!graph.ContainsKey(origin))
{
return(false);
}
// If there are no incoming roads to the destination
if (!graph.InverseGraph.ContainsKey(destination))
{
return(false);
}
if (origin == destination)
{
// correct path is empty
return(true);
}
DijstraSearchItem currentSearch = new DijstraSearchItem(origin, null, null, 0);
IntervalHeap <DijstraSearchItem> heap = new IntervalHeap <DijstraSearchItem>();
var itemLookup = new Dictionary <string, DijstraSearchItem>();
heap.Add(currentSearch);
itemLookup[origin] = currentSearch;
while (heap.Count != 0)
{
// find the least item and remove from heap
currentSearch = heap.FindMin();
if (currentSearch.Distance > upperSearchLimitInMeters)
{
return(false);
}
heap.DeleteMin();
if (currentSearch.Id == destination)
{
break;
}
if (graph.ContainsKey(currentSearch.Id))
{
foreach (var road in graph[currentSearch.Id])
{
// calculate new possible distance
var alt = currentSearch.Distance + road.Length.DistanceInMeters;
var potentialNewItem = new DijstraSearchItem(road.End, road, currentSearch, alt);
if (itemLookup.ContainsKey(road.End))
{
DijstraSearchItem searchItem = itemLookup[road.End];
if (alt < searchItem.Distance)
{
itemLookup[road.End] = potentialNewItem;
heap.Add(potentialNewItem);
}
}
else
{
itemLookup[road.End] = potentialNewItem;
heap.Add(potentialNewItem);
}
}
}
}
if (currentSearch.Id != destination)
{
// Perhaps there is no path from origin to destination
// To throw exception or return false??
return(false);
}
// Trace back path
List <DirectedRoad> result = new List <DirectedRoad>();
DijstraSearchItem tracer = currentSearch;
while (tracer.PrevRoad != null)
{
result.Add(tracer.PrevRoad);
tracer = tracer.Prev;
}
result.Reverse();
if (result.First().Start != origin || result.Last().End != destination)
{
throw new Exception("connecting path does not correctly connect origin and destination");
}
path = result;
return(true);
}
//Creates the initial Scene point.
Generator_Sys()
{
rand = new System.Random();
graph = new RoadGraph <RoadNode>();
//Initialize the initial state.
//Adjacents.
graph.add(
new RoadNode(
true,
GameStateController.Instance.GetComponent <BuildingPrefabWrapper>().home,
randomSingleBuilding(),
RoadNode.EndType.Four,
RoadNode.EndType.Four,
new Vector2(1, 0)
));
graph.add(
new RoadNode(
false,
graph.GetNode(new Vector2(1, 0)).getblockNW(),
randomSingleBuilding(),
RoadNode.EndType.Four,
RoadNode.EndType.Four,
new Vector2(0, -1)
));
graph.add(
new RoadNode(
true,
randomSingleBuilding(),
graph.GetNode(new Vector2(1, 0)).getblockNW(),
RoadNode.EndType.Four,
RoadNode.EndType.Four,
new Vector2(-1, 0)
));
graph.add(
new RoadNode(
false,
randomSingleBuilding(),
graph.GetNode(new Vector2(1, 0)).getblockNW(),
RoadNode.EndType.Four,
RoadNode.EndType.Four,
new Vector2(0, 1)
));
//Verticals
graph.add(
new RoadNode(
true,
graph.GetNode(new Vector2(0, 1)).getblockNW(),
randomSingleBuilding(),
RoadNode.EndType.Four,
RoadNode.EndType.Four,
new Vector2(1, 2)
));
graph.add(
new RoadNode(
true,
graph.GetNode(new Vector2(0, -1)).getblockES(),
randomSingleBuilding(),
RoadNode.EndType.Four,
RoadNode.EndType.Four,
new Vector2(1, -2)
));
graph.add(
new RoadNode(
true,
randomSingleBuilding(),
graph.GetNode(new Vector2(0, -1)).getblockES(),
RoadNode.EndType.Four,
RoadNode.EndType.Four,
new Vector2(-1, -2)
));
graph.add(
new RoadNode(
true,
randomSingleBuilding(),
graph.GetNode(new Vector2(0, 1)).getblockNW(),
RoadNode.EndType.Four,
RoadNode.EndType.Four,
new Vector2(-1, 2)
));
//Fill in.
graph.add(
new RoadNode(
false,
graph.GetNode(new Vector2(1, 2)).getblockES(),
graph.GetNode(new Vector2(1, 0)).getblockES(),
RoadNode.EndType.Four,
RoadNode.EndType.Four,
new Vector2(2, 1)
));
graph.add(
new RoadNode(
false,
graph.GetNode(new Vector2(1, 0)).getblockES(),
graph.GetNode(new Vector2(1, -2)).getblockES(),
RoadNode.EndType.Four,
RoadNode.EndType.Four,
new Vector2(1, -1)
));
graph.add(
new RoadNode(
false,
graph.GetNode(new Vector2(-1, 0)).getblockES(),
graph.GetNode(new Vector2(-1, -2)).getblockES(),
RoadNode.EndType.Four,
RoadNode.EndType.Four,
new Vector2(-2, -1)
));
graph.add(
new RoadNode(
false,
graph.GetNode(new Vector2(-1, 2)).getblockES(),
graph.GetNode(new Vector2(-1, 0)).getblockES(),
RoadNode.EndType.Four,
RoadNode.EndType.Four,
new Vector2(1, -2)
));
//Edges.
graph.add(graph.GetNode(new Vector2(0, 1)), graph.GetNode(new Vector2(1, 0)));
graph.add(graph.GetNode(new Vector2(1, 0)), graph.GetNode(new Vector2(0, 1)));
graph.add(graph.GetNode(new Vector2(1, 0)), graph.GetNode(new Vector2(0, -1)));
graph.add(graph.GetNode(new Vector2(0, -1)), graph.GetNode(new Vector2(1, 0)));
graph.add(graph.GetNode(new Vector2(0, -1)), graph.GetNode(new Vector2(-1, 0)));
graph.add(graph.GetNode(new Vector2(-1, 0)), graph.GetNode(new Vector2(0, -1)));
graph.add(graph.GetNode(new Vector2(-1, 0)), graph.GetNode(new Vector2(0, 1)));
graph.add(graph.GetNode(new Vector2(0, 1)), graph.GetNode(new Vector2(-1, 0)));
graph.add(graph.GetNode(new Vector2(0, 1)), graph.GetNode(new Vector2(1, 2)));
graph.add(graph.GetNode(new Vector2(1, 2)), graph.GetNode(new Vector2(0, 1)));
graph.add(graph.GetNode(new Vector2(1, 2)), graph.GetNode(new Vector2(2, 1)));
graph.add(graph.GetNode(new Vector2(2, 1)), graph.GetNode(new Vector2(1, 2)));
graph.add(graph.GetNode(new Vector2(2, 1)), graph.GetNode(new Vector2(1, 0)));
graph.add(graph.GetNode(new Vector2(1, 0)), graph.GetNode(new Vector2(2, 1)));
graph.add(graph.GetNode(new Vector2(1, 0)), graph.GetNode(new Vector2(1, -1)));
graph.add(graph.GetNode(new Vector2(1, -1)), graph.GetNode(new Vector2(1, 0)));
graph.add(graph.GetNode(new Vector2(1, -1)), graph.GetNode(new Vector2(-1, -2)));
graph.add(graph.GetNode(new Vector2(-1, -2)), graph.GetNode(new Vector2(1, -1)));
graph.add(graph.GetNode(new Vector2(-1, -2)), graph.GetNode(new Vector2(0, -1)));
graph.add(graph.GetNode(new Vector2(0, -1)), graph.GetNode(new Vector2(-1, -2)));
graph.add(graph.GetNode(new Vector2(0, -1)), graph.GetNode(new Vector2(-1, -2)));
graph.add(graph.GetNode(new Vector2(-1, -2)), graph.GetNode(new Vector2(0, -1)));
graph.add(graph.GetNode(new Vector2(-1, -2)), graph.GetNode(new Vector2(-2, -1)));
graph.add(graph.GetNode(new Vector2(-2, -1)), graph.GetNode(new Vector2(-1, -2)));
graph.add(graph.GetNode(new Vector2(-2, -1)), graph.GetNode(new Vector2(-1, 0)));
graph.add(graph.GetNode(new Vector2(-1, 0)), graph.GetNode(new Vector2(-2, -1)));
graph.add(graph.GetNode(new Vector2(-1, 0)), graph.GetNode(new Vector2(1, -2)));
graph.add(graph.GetNode(new Vector2(1, -2)), graph.GetNode(new Vector2(-1, 0)));
graph.add(graph.GetNode(new Vector2(1, -2)), graph.GetNode(new Vector2(-1, 2)));
graph.add(graph.GetNode(new Vector2(-1, 2)), graph.GetNode(new Vector2(1, -2)));
graph.add(graph.GetNode(new Vector2(-1, 2)), graph.GetNode(new Vector2(0, 1)));
graph.add(graph.GetNode(new Vector2(0, 1)), graph.GetNode(new Vector2(-1, 2)));
}
static void Main(string[] args)
{
string osmPath = "";
int eps = -1;
int minTraffic = -1;
bool showHelp = false;
OptionSet parameters = new OptionSet()
{
{ "osm=", "path to the map file", v => osmPath = v },
{ "eps=", "size of the eps-neighborhood to be considered (integer)", v => eps = Convert.ToInt32(v) },
{ "minTraffic=", "minimum traffic considered (integer)", v => minTraffic = Convert.ToInt32(v) },
{ "h|?|help", v => showHelp = v != null },
};
try
{
parameters.Parse(args);
}
catch (OptionException e)
{
Console.Write("TRoute: ");
Console.WriteLine(e.Message);
Console.WriteLine("Try `TRoute --help' for more information.");
return;
}
if (showHelp || string.IsNullOrEmpty(osmPath) || eps < 0 || minTraffic < 0)
{
ShowHelp(parameters);
return;
}
var osmFile = new OSMDB();
osmFile.Load(osmPath);
var roadGraph = new RoadGraph();
roadGraph.Build(osmFile);
// Getting all the nodes with traffic signals
Dictionary <double, OSMNode> tagsNodes = new Dictionary <double, OSMNode>();
foreach (var node in osmFile.Nodes)
{
foreach (var tag in node.Tags)
{
if (tag.Value.Equals("traffic_signals"))
{
if (!tagsNodes.Keys.Contains(node.Latitude + node.Longitude))
{
tagsNodes.Add(node.Latitude + node.Longitude, node);
}
}
}
}
var hotRoutes = new FlowScan().Run(roadGraph, eps, minTraffic);
// Saving GPX file of the Hot Route
HashSet <GPXPoint> listPoints;
HashSet <GPXTrackSegment> listSegments;
GPXTrackSegment segTrack;
List <GPXTrack> track = new List <GPXTrack>();
GPXTrack tr;
//Console.WriteLine(hotRoutes.Count);
foreach (var hr in hotRoutes)
{
//Console.WriteLine("Number segs: " + hr.Segments.Count);
listSegments = new HashSet <GPXTrackSegment>();
foreach (var seg in hr.Segments)
{
listPoints = new HashSet <GPXPoint>();
foreach (var segInner in seg.Geometry.Segments)
{
GPXPoint start;
if (tagsNodes.Keys.Contains(segInner.StartPoint.Latitude + segInner.StartPoint.Longitude))
{
OSMNode osmNode = tagsNodes[segInner.StartPoint.Latitude + segInner.StartPoint.Longitude];
start = new GPXPoint()
{
Id = osmNode.ID, Latitude = segInner.StartPoint.Latitude,
Longitude = segInner.StartPoint.Longitude, TrafficSignal = true
};
}
else
{
OSMNode osmNode = osmFile.Nodes.ToList().First(x => x.Latitude == segInner.StartPoint.Latitude &&
x.Longitude == segInner.StartPoint.Longitude);
start = new GPXPoint()
{
Id = osmNode.ID, Latitude = segInner.StartPoint.Latitude,
Longitude = segInner.StartPoint.Longitude, TrafficSignal = false
};
}
GPXPoint end;
if (tagsNodes.Keys.Contains(segInner.EndPoint.Latitude + segInner.EndPoint.Longitude))
{
OSMNode osmNode = tagsNodes[segInner.EndPoint.Latitude + segInner.EndPoint.Longitude];
end = new GPXPoint()
{
Id = osmNode.ID, Latitude = segInner.EndPoint.Latitude,
Longitude = segInner.EndPoint.Longitude, TrafficSignal = true
};
}
else
{
OSMNode osmNode = osmFile.Nodes.ToList().First(x => x.Latitude == segInner.EndPoint.Latitude &&
x.Longitude == segInner.EndPoint.Longitude);
end = new GPXPoint()
{
Id = osmNode.ID, Latitude = segInner.EndPoint.Latitude,
Longitude = segInner.EndPoint.Longitude, TrafficSignal = false
};
}
listPoints.Add(start);
listPoints.Add(end);
}
segTrack = new GPXTrackSegment(listPoints, seg.AvgSpeed, seg.Speed, seg.Id);
// passing the traffic
segTrack.Traffic = seg.Traffic;
listSegments.Add(segTrack);
}
tr = new GPXTrack();
tr.Segments.AddRange(listSegments);
track.Add(tr);
}
// Bucket Information
GPXTrack tBucket = new GPXTrack();
GPXPoint pBucket = new GPXPoint(0, 0, 0, false);
GPXTrackSegment sBucket = new GPXTrackSegment();
var bucketInfo = osmFile.Nodes.ToList().Find(x => x.ID == 0);
if (bucketInfo != null)
{
pBucket.StartBucket = TimeSpan.Parse(bucketInfo.Tags.First().Value);
pBucket.EndBucket = TimeSpan.Parse(bucketInfo.Tags.Last().Value);
}
sBucket.Nodes.Add(pBucket);
tBucket.Segments.Add(sBucket);
var gpx = new GPXDocument()
{
Tracks = track
};
gpx.Tracks.Add(tBucket);
gpx.Save("mapWithHotRoutes.gpx");
}
public override void DoStep(double msInterval)
{
try
{
if (!_located)
{
LocateAgent();
}
//Длина пути, которая может быть пройдена за один шаг моделирования
double step_lenght = msInterval / CurrentSpeed;
double tmp = _currentLength;
double route_length = 0;
for (int i = 1; i < RouteList.Count; i++)
{
//Поиск дороги
string pathData = RoadGraph.GetEdgeData(RouteList[i - 1], RouteList[i]);
PathFigure road = GetPathFigureFromString(pathData);
if (road == null)
{
throw new InvalidCastException("Не удалось десериализовать часть пути из строки (" + pathData + ")");
}
//Расчет длины маршрута и данного участка
double road_length = CalculatePathLength(road);
route_length += road_length;
//Если этот участок еще не пройден
if (_currentLength < road_length)
{
//Если участок не будет пройден за этот шаг
if (_currentLength + step_lenght + Size.X / 2 < road_length)
{
_currentLength += step_lenght;
Position = CalculateCoordinateLocationOfCarrige();
break;
}
else if (tmp + step_lenght + Size.X / 2 > road_length)
{
if (tmp + Size.X / 2 <= road_length)
{
if (RouteList[i].IsServicePoint)
{
//Паркуемся
if (tmp != road_length - Size.X / 2)
{
_currentLength += road_length - tmp - Size.X / 2;
Position = CalculateCoordinateLocationOfCarrige();
}
//Работаем с сервисом
if (!ReadyToIOOperation)
{
var service = _services.FirstOrDefault(s => s.Id == RouteList[i].ServiceId);
if (service == null)
{
throw new ArgumentException("Service not found, service id = " + RouteList[i].ServiceId);
}
service.AddAgent(this);
ReadyToIOOperation = true;
//if (service is StopService)
//{
// (service as StopService).OpenInputPoints(Id);
// ReadyToIOOperation = true;
//}
}
if (_go)
{
ReadyToIOOperation = false;
_go = false;
}
if (ReadyToIOOperation)
{
return;
}
}
if (RouteList.Count > i + 1 && i > 0 && RouteList[i + 1].Equals(RouteList[i - 1]))
{
Revers(route_length);
}
}
_currentLength += step_lenght;
Position = CalculateCoordinateLocationOfCarrige();
}
}
else
{
if (i == RouteList.Count - 1)
{
_currentLength += step_lenght;
RouteList.Clear();
}
tmp -= road_length;
}
}
}
catch (Exception ex)
{
Console.WriteLine(string.Format("Error in Thread! Agent Id [{0}]", Id));
Console.WriteLine(ex.ToString());
RouteList.Clear();
}
}
private Point CalculateCoordinateLocationOfCarrige()
{
double tmp = _currentLength;
for (int i = 1; i < RouteList.Count; i++)
{
//Поиск дороги
string pathData = RoadGraph.GetEdgeData(RouteList[i - 1], RouteList[i]);
PathFigure road = PathGeometry.CreateFromGeometry(PathGeometry.Parse(pathData)).Figures[0];
//Если дорога не найдена
if (road == null)
{
CalculateAngle(Position, RouteList[i].Location);
Position = RouteList[i].Location;
return(new Point());
}
//Если найдена
double road_lenght = CalculatePathLength(road);
//Если этот участок уже пройден, то смотрим следующий
if (tmp > road_lenght)
{
tmp -= road_lenght;
continue;
}
for (int g = 0; g < road.Segments.Count; g++)
{
double x = 0, y = 0, t = 0, seg_l = 0;
Point startPoint = road.StartPoint;
if (road.Segments[g] is BezierSegment)
{
BezierSegment seg = road.Segments[g] as BezierSegment;
seg_l = CalculatePathLength(new PathFigure(startPoint, new List <PathSegment>()
{
seg
}, false));
if (seg_l < tmp)
{
tmp -= seg_l;
startPoint = seg.Point3;
continue;
}
t = tmp / seg_l;
x = (1 - t) * (1 - t) * (1 - t) * startPoint.X + (1 - t) * (1 - t) * 3 * t * seg.Point1.X + (1 - t) * 3 * t * t * seg.Point2.X + t * t * t * seg.Point3.X;
y = (1 - t) * (1 - t) * (1 - t) * startPoint.Y + (1 - t) * (1 - t) * 3 * t * seg.Point1.Y + (1 - t) * 3 * t * t * seg.Point2.Y + t * t * t * seg.Point3.Y;
}
else if (road.Segments[g] is LineSegment)
{
LineSegment seg = road.Segments[g] as LineSegment;
seg_l = CalculatePathLength(new PathFigure(startPoint, new List <PathSegment>()
{
seg
}, false));
if (seg_l < tmp)
{
tmp -= seg_l;
startPoint = seg.Point;
continue;
}
t = tmp / seg_l;
x = seg.Point.X * t + startPoint.X * (1 - t);
if (startPoint.X == seg.Point.X)
{
y = startPoint.Y + (seg.Point.Y - startPoint.Y) * t;
}
else
{
y = (x - startPoint.X) * (seg.Point.Y - startPoint.Y) / (seg.Point.X - startPoint.X) + startPoint.Y;
}
}
else if (road.Segments[g] is PolyLineSegment)
{
PolyLineSegment seg = road.Segments[g] as PolyLineSegment;
int l;
for (l = 0; l < seg.Points.Count; l++)
{
seg_l = Math.Sqrt(Math.Pow(startPoint.X - seg.Points[l].X, 2) + Math.Pow(startPoint.Y - seg.Points[l].Y, 2));
if (seg_l < tmp)
{
tmp -= seg_l;
startPoint = seg.Points[l];
continue;
}
}
t = tmp / seg_l;
x = seg.Points[l].X * t + startPoint.X * (1 - t);
y = (x - startPoint.X) * (seg.Points[l].Y - startPoint.Y) / (seg.Points[l].X - startPoint.X) + startPoint.Y;
}
CalculateAngle(Position, new Point(x, y));
return(new Point(x, y));
}
}
return(new Point());
}
