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()); }