public bool IsTowardTrafficLight(Car c)
{
RectangleF rect = GetRectangle();
Rectangle4P r4p = new Rectangle4P(rect);
PointF nearestPoint = r4p.GetNearestPointInEdge(c.Position);
//
double radErrMargin = Calc.DegreeToRadians(ErrorMargin);
double angC_SB = Calc.NormalizeRadian(Calc.GetAngleOfLineBetweenTwoPoints(c.Position, nearestPoint));
//
if (Calc.IsInRange(angC_SB - (radErrMargin / 2), angC_SB + (radErrMargin / 2), Calc.NormalizeRadian(c.Angle)))
{
return(true);
}
return(false);
}
public bool IsAlligned(Car c)
{
RectangleF rect = GetRectangle();
Rectangle4P r4p = new Rectangle4P(rect);
PointF nearestPoint = r4p.GetNearestPointInEdge(c.Position);
//
double radErrMargin = Calc.DegreeToRadians(ErrorMargin);
double angSB_C = Calc.NormalizeRadian(Calc.GetAngleOfLineBetweenTwoPoints(nearestPoint, c.Position));
double tAngle = Calc.NormalizeRadian(Calc.DegreeToRadians(Angle));
double iAngle = Calc.NormalizeRadian(Calc.DegreeToRadians(Angle) - Math.PI);
//
if (Calc.IsInRange(tAngle - (radErrMargin / 2), tAngle + (radErrMargin / 2), angSB_C) ||
Calc.IsInRange(iAngle - (radErrMargin / 2), iAngle + (radErrMargin / 2), angSB_C))
{
return(true);
}
return(false);
}
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 virtual void Update()
{
// Decrease Car Speed
Speed -= 0.01f;
if (Speed < 0.00f)
{
Speed = 0.00f;
}
// Secure Speed Distance
SecureDistance = (Speed * SDRatio) + MSecureDistance;
// Position Changes
PointF IPos = new PointF(0.00f, 0.00f);
ObjectiveSpeed = MaximumSpeed;
// AI
if (!UserControl)
{
IPos = DoAI();
}
// User Control
double UpAngleRad = Calc.DegreeToRadians(270);
const double _90degreeRad = (Math.PI / 2);
if (UserControl)
{
if (Control.IsSomeButtonPressed())
{
SetObjectiveSpeed(MaximumSpeed);
}
else
{
SetObjectiveSpeed(0.00f);
}
//
if (Control.Up)
{
double angNormalized = Calc.NormalizeRadian(UpAngleRad);
IPos.X += (float)(Math.Cos(angNormalized) * Speed);
IPos.Y += (float)(Math.Sin(angNormalized) * Speed);
}
if (Control.Right)
{
double angNormalized = Calc.NormalizeRadian(UpAngleRad + _90degreeRad);
IPos.X += (float)(Math.Cos(angNormalized) * Speed);
IPos.Y += (float)(Math.Sin(angNormalized) * Speed);
}
if (Control.Down)
{
double angNormalized = Calc.NormalizeRadian(UpAngleRad + (2 * _90degreeRad));
IPos.X += (float)(Math.Cos(angNormalized) * Speed);
IPos.Y += (float)(Math.Sin(angNormalized) * Speed);
}
if (Control.Left)
{
double angNormalized = Calc.NormalizeRadian(UpAngleRad + (3 * _90degreeRad));
IPos.X += (float)(Math.Cos(angNormalized) * Speed);
IPos.Y += (float)(Math.Sin(angNormalized) * Speed);
}
}
// Car Operations
for (int C = 0; C < CLogic.Cars.Count; C++)
{
if (CLogic.Cars[C].Id != Id)
{
// Current Car
Car cCar = CLogic.Cars[C];
// Test Colisions
if (GetRectangle().IntersectsWith(cCar.GetRectangle()))
{
Kill();
}
}
}
// Speed Balance
if (Speed < ObjectiveSpeed)
{
SpeedUp();
}
else if (Speed > ObjectiveSpeed)
{
Break();
}
// Work?
if (!Working)
{
IPos = new PointF(0, 0);
}
// Increase Pos
Position.X += IPos.X;
Position.Y += IPos.Y;
// Calc Angle
if (Point.Round(LastPosition) != Point.Round(Position))
{
Angle = Calc.GetAngleOfLineBetweenTwoPoints(LastPosition, Position);
}
// Store the last position
LastPosition = new PointF(Position.X, Position.Y);
}
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);
}