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 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)); } }
/// <summary> /// Calculate the angle between the two path points /// </summary> /// <returns></returns> public double CalculateAngle() { Angle = Calc.GetAngleOfLineBetweenTwoPoints(P1, P2); return(Angle); }
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); }