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);
}
/// <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 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 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);
}