/*[TestCase(35, 85, DirectionCar.North, 90, 60, 2, 3, 0, 90, 75)]
* [TestCase(45, 5, DirectionCar.West, 45, 90, 5, 0, 3, 35, 70)]
* [TestCase(35, 85, DirectionCar.North, 90, 60, 2, 2, 0, 102.5, 60)]*/
public void GetDirectionTest(double cX, double cY, DirectionCar direction, double dX, double dY, int road,
int destRoad, int intersection, double eX, double eY)
{
var roads = new[]
{
new Road(new Vector(35, 80), new Vector(35, 100), 10, 0),
new Road(new Vector(0, 75), new Vector(30, 75), 10, 0),
new Road(new Vector(40, 75), new Vector(90, 75), 10, 0),
new Road(new Vector(95, 70), new Vector(95, 10), 10, 0),
new Road(new Vector(40, 5), new Vector(90, 5), 10, 0),
new Road(new Vector(35, 10), new Vector(35, 70), 10, 0)
};
var intersections = new[]
{
new Intersection(new Vector(35, 75), 10),
new Intersection(new Vector(95, 75), 10),
new Intersection(new Vector(95, 5), 10),
new Intersection(new Vector(35, 5), 10)
};
var city = new CityBuilder()
.Road(roads[0])
.Road(roads[1])
.Road(roads[2])
.Road(roads[3])
.Road(roads[4])
.Road(roads[5])
.Intersection(intersections[0])
.Intersection(intersections[1])
.Intersection(intersections[2])
.Intersection(intersections[3])
.Build();
var car = new Car(1, CarModel.GetModel("TestModel"), new Vector(cX, cY), new List <Sensor>(), null, direction, 5, 5);
city.Cars.Add(car);
car.Destination = new Destination {
Location = new Vector(dX, dY), Road = roads[destRoad]
};
var carDestination = GPSSystem.GetDirection(car, intersections[intersection]);
Assert.AreEqual(roads[road], carDestination.Road);
Assert.AreEqual(eX, carDestination.Location.X, 0.01);
Assert.AreEqual(eY, carDestination.Location.Y, 0.01);
}
/// <summary>
/// Runs the main car logic. Needs to be subscribed to the MainLoop.
/// </summary>
public void Update()
{
// If we're not initialized, initialize!
if (!initialized)
{
Init();
}
// Update the passenger count.
Car.PassengerCount = Car.Passengers.Count;
// Update the current road with the road at our location.
Car.CurrentRoad = GPSSystem.GetRoad(Car.Location);
Car.CurrentIntersection = GPSSystem.FindIntersection(Car.Location);
if (Car.CurrentRoad == null && Car.CurrentIntersection == null)
{
if (!resetTarget)
{
resetTarget = true;
var closestRoad = GPSSystem.NearestRoad(Car.Location);
if (closestRoad != null && Car != null)
{
Car.CurrentTarget =
MathUtil.Distance(closestRoad.End, Car.Location) >
MathUtil.Distance(closestRoad.Start, Car.Location)
? closestRoad.Start
: closestRoad.End;
App.Console.Print($"[C{Car.Id}] Lost road, trying to get back on, targeting {Car.CurrentTarget}", Colors.Blue);
}
;
}
}
else if (resetTarget)
{
App.Console.Print($"[C{Car.Id}] Road found again", Colors.Blue);
resetTarget = false;
}
// Calculate the distance to the local target (usually the next intersection).
var distanceToTarget = MathUtil.Distance(Car.Location, Car.CurrentTarget);
// Calculate the distance to the destination.
var distanceToDestination = MathUtil.Distance(Car.Location, Car.Destination.Location);
// Calculate the relative yaw (in degrees).
var yaw = MathUtil.VectorToAngle(Car.Rotation, Car.Direction);
// Get the current velocity of the car.
var velocity = Car.Velocity;
// Create a variable to store the added rotation in this update call.
var addedRotation = 0.0;
var closeToDestination = CloseToDestination();
// Check if we're close to our target but not the destination.
if (distanceToTarget < 20 && !closeToDestination && !resetTarget)
{
// Check if we've not obtained a new target yet.
if (!obtainedNewTarget)
{
// Find the nearest intersection.
var intersection = GPSSystem.FindIntersection(GetClosestRoadPoint(Car.Location));
if (intersection == null)
{
return;
}
App.Console.Print($"[C{Car.Id}] Requesting new target from intersection {intersection.Location}...", Colors.Blue);
// Request the next target from the GPSSystem.
var target = GPSSystem.GetDirection(Car, intersection);
// Update our target.
newTarget = target.Location;
obtainedNewTarget = true;
var distance = Math.Round(MathUtil.Distance(newTarget, Car.Location));
App.Console.Print($"[C{Car.Id}] Obtained a new target {newTarget} ({distance}m away)", Colors.Blue);
}
}
else
{
obtainedNewTarget = false;
}
// Check if we are turning.
if (turning > 0)
{
turning--;
// Check if we locked on the new target yet.
if (newTarget.X > -1)
{
// Lock on the new target and reset newTarget.
Car.CurrentTarget = newTarget;
newTarget = new Vector(-1, -1);
App.Console.Print($"[C{Car.Id}] Locked on to target", Colors.Blue);
}
// Call the handle function to turn.
HandleTurn(ref velocity, ref yaw, ref addedRotation);
}
// Check if we're still turning around.
else if (flipping)
{
// Stop turning around.
flipping = false;
App.Console.Print($"[C{Car.Id}] Turn-around done", Colors.Blue);
}
// Check if we're on an intersection.
if (Car.CurrentIntersection != null)
{
// Reset our turn timer.
turning = 20;
}
else
{
// Check if we're not close to the destination.
if (!closeToDestination)
{
// If we're still approaching, stop approaching.
if (approach)
{
approach = false;
App.Console.Print($"[C{Car.Id}] Approach done", Colors.Blue);
}
// Call the handle functions to stay in the lane and accelerate/deccelerate.
HandleStayInLane(ref velocity, ref yaw, ref addedRotation);
HandleAccelerate(ref velocity, ref distanceToTarget);
}
else
{
// If we're not approaching, start approaching.
if (!approach)
{
approach = true;
App.Console.Print($"[C{Car.Id}] Initiating approach", Colors.Blue);
}
// Call the handle function to approach the target.
HandleApproachTarget(ref velocity, ref yaw, ref addedRotation, ref distanceToDestination);
}
}
// Update the car's velocity with the result of the handle functions:
// Temporarily store the current speed.
var speed = velocity.Length;
// Rotate the velocity based on the addedRotation this tick.
velocity = MathUtil.RotateVector(velocity, -addedRotation);
// Normalize the velocity and multiply with the speed to make sure it stays the same.
velocity.Normalize();
velocity = Vector.Multiply(velocity, speed);
// Actually update the car's velocity.
Car.Velocity = velocity;
// Calculate the rotation by normalizing the velocity.
var rotation = new Vector(velocity.X, velocity.Y);
rotation.Normalize();
// If the rotation vector is invalid or the car is not moving, set the rotation to the absolute direction.
if (rotation.Length < 0.9 || double.IsNaN(rotation.Length) || velocity.Length < 0.1)
{
rotation = Car.Direction.GetVector();
}
// Actually update the rotation and update the absolute direction.
Car.Rotation = rotation;
Car.Direction = DirectionCarMethods.Parse(rotation);
// Update the car's location by adding the velocity to it.
Car.Location = Vector.Add(Car.Location, Car.Velocity);
Car.DistanceTraveled += Car.Velocity.Length;
}