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);
}
public PointF DoAI()
{
// IPos
PointF IPos = new PointF(0.00f, 0.00f);
// Immediate Tasks
bool hasImmediate = ImmediateTasks.Count != 0;
if (hasImmediate)
{
Task t = ImmediateTasks.Peek();
PointF diff = DoTask(t);
IPos.X += diff.X;
IPos.Y += diff.Y;
}
// Solve Tasks Queue
if (TasksQueue.Count > 0 && !hasImmediate)
{
Task t = TasksQueue.Peek();
PointF diff = DoTask(t);
IPos.X += diff.X;
IPos.Y += diff.Y;
}
// Nomadic Mode
if (Nomadic)
{
Path cp = GetBelongingPath();
if (cp != null)
{
if (TasksQueue.Count == 0 && !hasImmediate)
{
PointF endOfPath = cp.GetPathCornerBasedOnDirection();
float dist = Calc.Modulus(Calc.Magnitude(endOfPath, Position));
if (dist < cp.ErrorMargin)
{
Path ip = cp.ParentPathGroup.GetWayWithDir(!cp.Direction);
PointF nearestCorner = ip.GetNearestCorner(Position);
Task t = CreateTask(nearestCorner, TaskType.Queue);
EnqueueTask(t);
}
else
{
//Task t = CreateTask(endOfPath, TaskType.Immediate);
//ImmediateTasks.Push(t);
//
Task t = CreateTask(endOfPath, TaskType.Queue);
EnqueueTask(t);
}
}
}
}
// Path Info
Path cCarPath = GetBelongingPath();
int cCarPId = cCarPath != null ? cCarPath.ID : -1;
// Speed Bump
foreach (SpeedBump sb in CLogic.SpeedBumps)
{
if (sb.IsAlligned(this))
{
if (sb.IsTowardSpeedBump(this))
{
float dist = sb.DistanceOfCar(this);
//
if (dist < SecureDistance)
{
SetObjectiveSpeed(sb.DesiredSpeed);
}
}
}
}
////////////////////////////////////////// IT'S NOT WORKING CORRECTLY! //////////////////////////////////////////////////////
// Change Way (if there's another free way)
if (cCarPath != null)
{
Car cNearest = cCarPath.GetNearestCarInPath(CLogic, Position, Id);
if (cNearest != null)
{
float dist = Calc.Modulus(Calc.Magnitude(Position, cNearest.Position));
if (dist < SecureDistance) // <--------------------------------------- TODO
{
List <Path> availablePaths = cCarPath.ParentPathGroup.GetWaysWithDir(cCarPath.Direction);
foreach (Path p in availablePaths)
{
if (p.ID == cCarPath.ID)
{
continue;
}
//
PointF nLinePos = Calc.PointLineNormalizedIntersection(Position, p.P1, p.P2);
Car nearestAnotherPath = p.GetNearestCarInPath(CLogic, nLinePos);
if (nearestAnotherPath == null)
{
//
PointF futurePoint = new PointF(nLinePos.X + (float)((Math.Cos(Angle) * Speed)),
nLinePos.Y + (float)((Math.Sin(Angle) * SecureDistance)));
// Go TO Another Path
Task task = CreateTask(futurePoint, TaskType.Immediate);
ImmediateTasks.Push(task);
TasksQueue.Clear();
break;
}
//
float distCarAnotherPath = Calc.Modulus(Calc.Magnitude(Position, nearestAnotherPath.Position));
if (distCarAnotherPath > SecureDistance)
{
//
PointF futurePoint = new PointF(nLinePos.X + (float)((Math.Cos(Angle) * Speed)),
nLinePos.Y + (float)((Math.Sin(Angle) * SecureDistance)));
// Go TO Another Path
Task task = CreateTask(futurePoint, TaskType.Immediate);
ImmediateTasks.Push(task);
TasksQueue.Clear();
break;
}
}
}
}
}
// Car Operations
for (int C = 0; C < CLogic.Cars.Count; C++)
{
if (CLogic.Cars[C].Id != Id)
{
// Current Car
Car cCar = CLogic.Cars[C];
// Error margin
const float errAngle = 30.0f;
double errAngleRad = Calc.NormalizeRadian(Calc.DegreeToRadians(errAngle));
// Vars
double angAc = Calc.NormalizeRadian(cCar.Angle);
double angAcInv = Calc.NormalizeRadian(cCar.Angle + Math.PI);
double angTc = Calc.NormalizeRadian(Angle);
double angAcTc = Calc.NormalizeRadian(Calc.GetAngleOfLineBetweenTwoPoints(cCar.Position, Position));
double angTcAc = Calc.NormalizeRadian(Calc.GetAngleOfLineBetweenTwoPoints(Position, cCar.Position));
float distBoth = Calc.Modulus(Calc.Magnitude(Position, cCar.Position));
// Test if the other car is going toward this car
if (Calc.IsInRange(angAcTc - (errAngleRad / 2.00d), angAcTc + (errAngleRad / 2.00d), angAc))
{
// Test if the other car is going in the same direction
if (Calc.IsInRange(angTc - (errAngleRad / 2.00d), angTc + (errAngleRad / 2.00d), angAcInv))
{
if (distBoth < SecureDistance)
{
//Break();
SetObjectiveSpeed(0.00f);
}
}
}
// Test if this car is going toward another car (few FOV to avoid the car speed down when the car is comming in the oposite way)
//if (Calc.IsInRange(angTcAc - (errAngleRad / 2.00d), angTcAc + (errAngleRad / 2.00d), angTc))
//{
// if (distBoth < SecureDistance)
// {
// //Break();
// SetObjectiveSpeed(0.00f);
// }
//}
if (Calc.IsInRange(angTcAc - (Calc.DegreeToRadians(FOV) / 2.00d), angTcAc + (Calc.DegreeToRadians(FOV) / 2.00d), angTc))
{
Path bpcCar = cCar.GetBelongingPath();
if (bpcCar != null && GetBelongingPath() != null)
{
if (distBoth < SecureDistance && bpcCar.ID == GetBelongingPath().ID)
{
//Break();
SetObjectiveSpeed(0.00f);
}
}
else
{
if (distBoth < SecureDistance)
{
//Break();
SetObjectiveSpeed(0.00f);
}
}
}
}
}
// Traffic Lights
foreach (TrafficLightGroup tfg in CLogic.TrafficLights)
{
foreach (TrafficLight tl in tfg.TLs)
{
if (tl.IsAlligned(this))
{
if (tl.IsTowardTrafficLight(this))
{
if (tl.Status == TrafficLightStatus.Red)
{
float dist = tl.DistanceOfCar(this);
//
if (dist < SecureDistance)
{
SetObjectiveSpeed(0.00f);
}
}
}
}
}
}
return(IPos);
}
