public Car GetNearestCarInPath(Logic cl, PointF pos, int ignoreId)
{
Car cOut = null;
float dist = 9999999999999.0f;
//
foreach (Car c in cl.Cars)
{
Path pTmp = c.GetBelongingPath();
if (pTmp != null)
{
if (pTmp.ID == ID)
{
if (Calc.Magnitude(pos, c.Position) < dist)
{
if (c.Id == ignoreId)
{
continue;
}
cOut = c;
dist = Calc.Magnitude(pos, c.Position);
}
}
}
}
//
return(cOut);
}
public void Update()
{
foreach (Car c in Cars)
{
// Keep In Path
if (c.KeepInPath)
{
Path np = GetNearestPath(c.Position);
PointF pathNearestPoint = np.GetNearestPathPoint(c.Position);
//
float dist = Calc.Modulus(Calc.Magnitude(c.Position, pathNearestPoint));
if (dist > c.ErrorMargin)
{
if (c.ImmediateTasks.Count == 0)
{
Task t = c.CreateTask(pathNearestPoint, TaskType.Immediate);
c.ImmediateTasks.Push(t);
}
}
}
// Update the Traffic Light
foreach (TrafficLightGroup tfg in TrafficLights)
{
tfg.DoStep();
}
// Update the Car Behavior
c.Update();
}
}
public PointF GetNearestPointInEdge(PointF p)
{
float dist = Calc.Modulus(Calc.Magnitude(P1, p));
PointF np = P1;
//
for (int C = 1; C <= 4; C++)
{
//Point CP = GetPointByNumber(C);
////
//float cDist = Calc.Modulus(Calc.Magnitude(CP, p));
//if (cDist < dist)
//{
// dist = cDist;
// np = CP;
//}
Line ln = new Line();
ln.P1 = Point.Round(GetPointByNumber(C));
ln.P2 = Point.Round(GetPointByNumber(C + 1 == 5 ? 1 : C + 1));
//
PointF intersectionPos = Calc.PointLineNormalizedIntersection(p, ln.P1, ln.P2);
float cDist = Calc.Modulus(Calc.DistancePointLine(p, ln.P1, ln.P2));
if (cDist < dist)
{
dist = cDist;
np = intersectionPos;
}
}
//
return(np);
}
public Path GetNearestPath(PointF pos)
{
Path np = null;
float dist = 0.00f;
bool first = true;
//
foreach (PathGroup pg in PathGroups)
{
foreach (Path p in pg.Paths)
{
PointF nearestPath = p.GetNearestPathPoint(pos);
float distTmp = Calc.Modulus(Calc.Magnitude(pos, nearestPath));
//
if (first)
{
first = false;
dist = distTmp;
np = p;
}
//
if (distTmp < dist)
{
dist = distTmp;
np = p;
}
}
}
//
return(np);
}
public bool IsInCorner(PointF pos)
{
if (Calc.Modulus(Calc.Magnitude(pos, P1)) <= ErrorMargin ||
Calc.Modulus(Calc.Magnitude(pos, P2)) <= ErrorMargin)
{
return(true);
}
return(false);
}
public float DistanceOfCar(Car c)
{
RectangleF rect = GetRectangle();
Rectangle4P r4p = new Rectangle4P(rect);
PointF nearestPoint = r4p.GetNearestPointInEdge(c.Position);
//
return(Calc.Magnitude(c.Position, nearestPoint));
}
/// <summary>
/// 0 = None
/// 1 = P1
/// 2 = P2
/// </summary>
/// <param name="pos"></param>
/// <returns></returns>
public int IsInWhichCorner(PointF pos)
{
if (Calc.Modulus(Calc.Magnitude(pos, P1)) <= ErrorMargin)
{
return(1);
}
if (Calc.Modulus(Calc.Magnitude(pos, P2)) <= ErrorMargin)
{
return(2);
}
return(0);
}
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 GetNearestCorner(PointF pos)
{
float distP1 = Calc.Modulus(Calc.Magnitude(pos, P1));
float distP2 = Calc.Modulus(Calc.Magnitude(pos, P2));
//
if (distP1 < distP2)
{
return(P1);
}
else
{
return(P2);
}
}
public Path GetBelongingPath()
{
//////////////////// TODO <------------- THE CAR NEED TO BE ALIGNED WITH THE STREET (SAME ANGLES)
foreach (PathGroup pg in CLogic.PathGroups)
{
foreach (Path p in pg.Paths)
{
if (Calc.Modulus(Calc.Magnitude(p.GetNearestPathPoint(Position), Position)) <= ErrorMargin)
{
return(p);
}
}
}
return(null);
}
/// <summary>
/// Calculate the part of the path the car is running
/// Returns -1 case is out of th epath
/// </summary>
/// <param name="c"></param>
/// <returns></returns>
public float PartOfPath(Car c)
{
bool isBetweenTheLine = Calc.DoesPointLineNormalizedIntersectionIsBetweenTheLine(c.Position, P1, P2);
if (!isBetweenTheLine)
{
return(-1.00f);
}
//
PointF intersectionPoint = Calc.PointLineNormalizedIntersection(c.Position, P1, P2);
float dist = Calc.Magnitude(c.Position, intersectionPoint);
if (dist > ErrorMargin)
{
return(-1.00f);
}
//
return(Calc.Magnitude(P1, c.Position));
}
public PointF GetNearestPoint(PointF p)
{
float dist = Calc.Modulus(Calc.Magnitude(P1, p));
PointF np = P1;
//
for (int C = 1; C <= 4; C++)
{
PointF CP = GetPointByNumber(C);
//
float cDist = Calc.Modulus(Calc.Magnitude(CP, p));
if (cDist < dist)
{
dist = cDist;
np = CP;
}
}
//
return(np);
}
public PointF GetNearestPathPoint(PointF pos)
{
bool isBetweenLineSegment = Calc.DoesPointLineNormalizedIntersectionIsBetweenTheLine(pos, P1, P2);
if (isBetweenLineSegment)
{
return(Calc.PointLineNormalizedIntersection(pos, P1, P2));
}
else
{
float distCP1 = Calc.Modulus(Calc.Magnitude(pos, P1));
float distCP2 = Calc.Modulus(Calc.Magnitude(pos, P2));
if (distCP1 < distCP2)
{
return(P1);
}
else
{
return(P2);
}
}
}
public PointF GetNearestPointBetweenTwoRectangles(Rectangle4P r4p)
{
float dist = Calc.Modulus(Calc.Magnitude(P1, r4p.P1));
PointF np = P1;
//
for (int C = 1; C <= 4; C++)
{
PointF CP = GetPointByNumber(C);
//
for (int D = 1; D <= 4; D++)
{
float cDist = Calc.Modulus(Calc.Magnitude(CP, r4p.GetPointByNumber(D)));
if (cDist < dist)
{
dist = cDist;
np = CP;
}
}
}
//
return(np);
}
private void pictureBox1_Paint(object sender, PaintEventArgs e)
{
Graphics GOut = e.Graphics;
GOut.Clear(Color.Black);
if (CLogic == null)
{
return;
}
foreach (PathGroup pg in CLogic.PathGroups)
{
Point lastBegin = new Point(0, 0);
Point lastEnd = new Point(0, 0);
bool first = true;
foreach (Path p in pg.Paths)
{
// Draw the Path
GOut.DrawLine(new Pen(Color.White, 3), ProjectPoint(p.P1),
ProjectPoint(p.P2));
// Draw the direction
PointF cArrow = new PointF(p.P1.X, p.P1.Y);
double dirSE = Calc.GetAngleOfLineBetweenTwoPoints(p.P1, p.P2);
Color lineColor = Color.FromArgb(100, 255, 255, 255);
double inverseAngle = dirSE;
if (p.Direction == true)
{
inverseAngle -= Math.PI;
}
float diffAngle = 20.00f; // IN DEGREE
float lineSize = 20.00f;
float distSE = Calc.Modulus(Calc.Magnitude(p.P1, p.P2));
float distMagn = 30.0f;
while (true)
{
if (Calc.Modulus(Calc.Magnitude(cArrow, p.P1)) > distSE)
{
break;
}
//
double angleP1 = inverseAngle - Calc.DegreeToRadians(diffAngle);
double angleP2 = inverseAngle + Calc.DegreeToRadians(diffAngle);
PointF p1 = new PointF((float)(cArrow.X - (Math.Cos(angleP1) * lineSize)),
(float)(cArrow.Y - (Math.Sin(angleP1) * lineSize)));
PointF p2 = new PointF((float)(cArrow.X - (Math.Cos(angleP2) * lineSize)),
(float)(cArrow.Y - (Math.Sin(angleP2) * lineSize)));
//
GOut.DrawLine(new Pen(lineColor), ProjectPoint(Point.Round(cArrow)), ProjectPoint(Point.Round(p1)));
GOut.DrawLine(new Pen(lineColor), ProjectPoint(Point.Round(cArrow)), ProjectPoint(Point.Round(p2)));
//
cArrow.X += (float)(Math.Cos(dirSE) * distMagn);
cArrow.Y += (float)(Math.Sin(dirSE) * distMagn);
}
// Draw the line that belongs the pathgroup
if (first)
{
first = false;
}
else
{
GOut.DrawLine(new Pen(Color.Gray), ProjectPoint(lastBegin),
ProjectPoint(p.P1));
GOut.DrawLine(new Pen(Color.Gray), ProjectPoint(lastEnd),
ProjectPoint(p.P2));
}
//
lastBegin = p.P1;
lastEnd = p.P2;
}
}
foreach (SpeedBump sb in CLogic.SpeedBumps)
{
GOut.FillRectangle(new SolidBrush(Color.Orange), ProjectRectangle(Rectangle.Round(sb.GetRectangle())));
}
foreach (TrafficLightGroup tlg in CLogic.TrafficLights)
{
foreach (TrafficLight tl in tlg.TLs)
{
GOut.FillRectangle(new SolidBrush(tl.GetColor()), ProjectRectangle(Rectangle.Round(tl.GetRectangle())));
}
}
foreach (PathIntersectionData pid in CLogic.PathIntersections)
{
if (pid.IntersectionData.Intersects)
{
GOut.FillPie(new SolidBrush(Color.Red), new Rectangle(ProjectPoint(new Point((int)(pid.IntersectionPoint.X - 3), (int)(pid.IntersectionPoint.Y - 3))), new Size(6, 6)), 0, 360);
}
}
foreach (Car c in CLogic.Cars)
{
// Draw the rectangle
Point projectedPoint = ProjectPoint(Point.Round(c.Position));
Size projectedSize = ProjectSize(c.Size);
Rectangle carRect = new Rectangle((int)(projectedPoint.X - (projectedSize.Width / 2)),
(int)(projectedPoint.Y - (projectedSize.Height / 2)),
projectedSize.Width, projectedSize.Height);
GOut.FillRectangle(new SolidBrush(c.Color), carRect);
// Draw the FOV
float lineSize = 30.0f;
double halfAngleRad = Calc.DegreeToRadians(c.FOV / 2);
double p1Angle = c.Angle - halfAngleRad;
double p2Angle = c.Angle + halfAngleRad;
PointF p1Rad = new PointF((float)(c.Position.X + (Math.Cos(p1Angle) * lineSize)), (float)(c.Position.Y + (Math.Sin(p1Angle) * lineSize)));
PointF p2Rad = new PointF((float)(c.Position.X + (Math.Cos(p2Angle) * lineSize)), (float)(c.Position.Y + (Math.Sin(p2Angle) * lineSize)));
GOut.DrawLine(new Pen(Color.FromArgb(100, 255, 255, 255)), ProjectPoint(Point.Round(c.Position)), ProjectPoint(Point.Round(p1Rad)));
GOut.DrawLine(new Pen(Color.FromArgb(100, 255, 255, 255)), ProjectPoint(Point.Round(c.Position)), ProjectPoint(Point.Round(p2Rad)));
// Draw the Id
Font idFont = new System.Drawing.Font("Consolas", 12);
SizeF stringSize = GOut.MeasureString(c.Id.ToString(), idFont);
GOut.DrawString(c.Id.ToString(), idFont, new SolidBrush(Color.White),
new PointF(carRect.X + ((carRect.Width / 2) - (stringSize.Width / 2)), carRect.Y - stringSize.Height));
}
}
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);
}
/// <summary>
/// Calculates the path lenght
/// </summary>
/// <returns></returns>
public float GetPathLenght()
{
return(Calc.Magnitude(P1, P2));
}