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