public PointF GetEndOfPathPointToward()
{
Path np = GetBelongingPath();
if (np == null)
{
return(new PointF(Position.X, Position.Y));
}
//
double streetAngle = np.CalculateAngle();
double invStreetAngle = Calc.NormalizeRadian(streetAngle + Math.PI);
//
double diffStraight = Math.Abs(Angle - streetAngle);
double diffInvStraight = Math.Abs(Angle - invStreetAngle);
//
if (diffStraight < diffInvStraight)
{
return(np.P2);
}
else
{
return(np.P1);
}
}
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);
}
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);
}