public PointF DoTask(Task t)
{
PointF IPos = new PointF(0, 0);
float dist = Calc.Magnitude(Position, t.Objective);
//
if (t.Unread == true)
{
t.Unread = false;
//
PushOutput("Started going from " + Logic.PointToString(Position) + " to " + Logic.PointToString(t.Objective));
}
//
if (dist <= ErrorMargin)
{
if (t.Type == TaskType.Immediate)
{
if (ImmediateTasks.Count > 0)
{
ImmediateTasks.Pop();
PushOutput(" The car reached the objetive " + Logic.PointToString(t.Objective));
}
else
{
// Error
}
}
else if (t.Type == TaskType.Queue)
{
TasksQueue.Dequeue();
}
}
else
{
double ang = Calc.GetAngleOfLineBetweenTwoPoints(Position, t.Objective);
//
IPos.X += (float)(Math.Cos(ang) * Speed);
IPos.Y += (float)(Math.Sin(ang) * Speed);
//
if (dist > SecureDistance)
{
//SpeedUp();
SetObjectiveSpeed(MaximumSpeed);
}
else
{
SetObjectiveSpeed(0.5f);
}
}
//
return(IPos);
}