public Destination Calculate(long callerId, Destination carDestination, Destination endDestination)
{
var network = NodeNetwork.NodeNetwork.GetInstance();
var startNode = new Node(GPSSystem.FindIntersection(carDestination.Location).Location);
var endNodes = AlgorithmTools.IntersectionTupleToNodeTuple(
AlgorithmTools.GetIntersectionOrderForRoadSide(endDestination.Road, endDestination.Location));
if (startNode.Equals(endNodes.Item1))
{
#if DEBUG
AlgorithmDebuggerWindow.Instance.AddNetworkResult(_debuggerIndex++.ToString(), network,
startNode, endNodes.Item2, endNodes);
#endif
return(endDestination);
}
AssignNodeValues(ref network, ref startNode);
var nextNode = FindNextNodeOnBestRoute(ref network, endNodes.Item1);
#if DEBUG
AlgorithmDebuggerWindow.Instance.AddNetworkResult("DY" + _debuggerIndex++, network,
startNode, nextNode, endNodes);
#endif
var roadX = (startNode.PositionX - nextNode.PositionX) / 2.0 + nextNode.PositionX;
var roadY = (startNode.PositionY - nextNode.PositionY) / 2.0 + nextNode.PositionY;
return(new Destination
{
Location = new Vector(nextNode.PositionX, nextNode.PositionY),
Road = GPSSystem.NearestRoad(new Vector(roadX, roadY))
});
}
public Destination Calculate(long callerId, Destination carDestination, Destination endDestination)
{
var network = NodeNetwork.NodeNetwork.GetInstance();
var startNode = new Node(GPSSystem.FindIntersection(carDestination.Location).Location);
var endNodes = AlgorithmTools.IntersectionTupleToNodeTuple(
AlgorithmTools.GetIntersectionOrderForRoadSide(endDestination.Road, endDestination.Location));
Node nextNode;
double roadX, roadY;
if (startNode.Equals(endNodes.Item1))
{
#if DEBUG
AlgorithmDebuggerWindow.Instance.AddNetworkResult(_debuggerIndex++.ToString(), network,
startNode, endNodes.Item2, endNodes);
#endif
return(endDestination);
}
if (_calculationCache.ContainsKey(callerId))
{
var tuple = _calculationCache[callerId];
if (tuple.Item1.Equals(endDestination))
{
var path = tuple.Item2;
var thisNode = path.Single(n => n.Equals(startNode));
nextNode = thisNode.ConnectedTo;
#if DEBUG
AlgorithmDebuggerWindow.Instance.AddNetworkResult(_debuggerIndex++.ToString(), network,
startNode, nextNode, endNodes);
#endif
roadX = (startNode.PositionX - nextNode.PositionX) / 2.0 + nextNode.PositionX;
roadY = (startNode.PositionY - nextNode.PositionY) / 2.0 + nextNode.PositionY;
return(new Destination
{
Location = new Vector(nextNode.PositionX, nextNode.PositionY),
Road = GPSSystem.NearestRoad(new Vector(roadX, roadY))
});
}
_calculationCache.Remove(callerId);
}
var endNode = endNodes.Item1;
nextNode = CalculatePathNextNode(callerId, ref network, ref startNode, ref endNode, endDestination);
#if DEBUG
AlgorithmDebuggerWindow.Instance.AddNetworkResult("AS" + _debuggerIndex++, network,
startNode, nextNode, endNodes);
#endif
roadX = (startNode.PositionX - nextNode.PositionX) / 2.0 + nextNode.PositionX;
roadY = (startNode.PositionY - nextNode.PositionY) / 2.0 + nextNode.PositionY;
return(new Destination
{
Location = new Vector(nextNode.PositionX, nextNode.PositionY),
Road = GPSSystem.NearestRoad(new Vector(roadX, roadY))
});
}
/// <summary>
/// Calculates the path to the customergroup using Dijkstra algorithm
/// </summary>
/// <param name="start">start path</param>
/// <param name="group">group customers</param>
/// <returns>list with the roads to take</returns>
public static List <Road> CalculateRouteToCustomerGroup(Vector start, CustomerGroup group)
{
var startRoad = GPSSystem.NearestRoad(start);
var endRoad = GPSSystem.NearestRoad(group.Location);
var hitEnd = false;
var listPaths = new List <Path>();
// if the starroad equals to the endRoad
if (startRoad == endRoad)
{
return(new List <Road> {
startRoad
});
}
;
//add the first two intersections of the startRoad to the PathList;
foreach (var item in GPSSystem.FindIntersectionsRoad(startRoad))
{
listPaths.Add(new Path(start, group.Location, item));
}
// loop until the end is found
while (hitEnd == false)
{
var newListPaths = new List <Path>();
foreach (var path in listPaths)
{
// new found intersections
var lastIntersectionOfPath = path.Intersections.Last();
var newFoundIntersections = GPSSystem.FindNextIntersections(lastIntersectionOfPath);
// add the path of every new intersetion to the new ListPath
foreach (var intersection in newFoundIntersections)
{
var intersections = new List <Intersection>(path.Intersections);
intersections.Add(intersection);
var newPath = new Path(start, group.Location, intersection);
newListPaths.Add(newPath);
// check if endLocation is between these intersections
if ((endRoad.End == intersection.Location || endRoad.Start == intersection.Location) && (endRoad.End == lastIntersectionOfPath.Location || endRoad.Start == lastIntersectionOfPath.Location))
{
return(newPath.PathToRoadList());
}
else
{
throw new Exception("roads are not horizontal or vertical");
}
}
}
// set the new list
listPaths = newListPaths;
}
return(null);
}
public CustomerGroupController(CustomerGroup group)
{
Group = group;
var road = GPSSystem.NearestRoad(group.Location);
MoveToNearestRoad(road);
Group.RoadsNear = GPSSystem.GetRoadsInRange(Group.Location, GroupRadius);
}
public override List <Road> CalculatePath(Vector start, CustomerGroup group)
{
var startRoad = GPSSystem.NearestRoad(start);
var endRoad = GPSSystem.NearestRoad(group.Location);
var listPaths = new List <Path>();
// if the startroad equals to the endRoad
if (startRoad == endRoad)
{
return(new List <Road> {
startRoad
});
}
;
//add the first two intersections of the startRoad to the PathList;
foreach (var item in GPSSystem.FindIntersectionsRoad(startRoad))
{
listPaths.Add(new Path(start, group.Location, item));
}
// order list
orderListByPriority(ref listPaths);
// loop until the end is found
while (true)
{
// new found intersections
var lastIntersectionOfPath = listPaths[0].Intersections.Last();
var newFoundIntersections = GPSSystem.FindNextIntersections(lastIntersectionOfPath);
// add the path of every new intersetion to the new ListPath
foreach (var intersection in newFoundIntersections)
{
var intersections = new List <Intersection>(listPaths[0].Intersections);
intersections.Add(intersection);
var newPath = new Path(start, group.Location, intersection);
listPaths.Add(newPath);
// check if endLocation is between these intersections
if ((endRoad.End == intersection.Location || endRoad.Start == intersection.Location) && (endRoad.End == lastIntersectionOfPath.Location || endRoad.Start == lastIntersectionOfPath.Location))
{
return(newPath.PathToRoadList());
}
}
listPaths.RemoveAt(0);
orderListByPriority(ref listPaths);
}
}
public void NearestRoadTest(int x1b, int y1b, int x1e, int y1e, int x2b, int y2b, int x2e, int y2e, int ex,
int ey)
{
var road1 = new Road(new Vector(x1b, y1b), new Vector(x1e, y1e), 20, 100);
var road2 = new Road(new Vector(x2b, y2b), new Vector(x2e, y2e), 20, 100);
var city = new CityBuilder()
.Road(road1)
.Road(road2)
.Build();
var road = GPSSystem.NearestRoad(new Vector(ex, ey));
Assert.AreEqual(road, road1);
}
public List <Road> PathToRoadList()
{
var listRoads = new List <Road>();
foreach (var road in City.Instance.Roads)
{
if (Intersections.FindAll(i => i.Location == road.Start)
.FindAll(i => i.Location == road.End).Count > 0)
{
listRoads.Add(road);
}
}
listRoads.Add(GPSSystem.NearestRoad(Start));
listRoads.Add(GPSSystem.NearestRoad(End));
return(listRoads);
}
public void TestLookForNearestRoad(double groupX, double groupY, bool road)
{
var building = new Building(new Vector(groupX, groupY), 8);
var road1 = new Road(new Vector(0, 50), new Vector(10, 50), 10, 100);
var road2 = new Road(new Vector(0, 20), new Vector(10, 20), 10, 100);
var city = new CityBuilder()
.Road(road1)
.Road(road2)
.Building(building)
.Build();
var group = new CustomerGroup(5, building, null);
var result = GPSSystem.NearestRoad(group.Location);
Assert.AreEqual(road ? road1 : road2, result);
}
public void Update()
{
var buitenRange = Group.Customers.Any(c => MathUtil.Distance(c.Location, Group.Location) > GroupRadius);
if (RequestedCar)
{
var car = City.Instance.Cars.Find(c => MathUtil.Distance(c.Location, Group.Location) < 20);
if (car == null || car.Passengers.Count > 0)
{
return;
}
Group.Customers.ForEach(customer => customer.Controller.Destroy());
MainScreen.AILoop.Unsubscribe(Update);
car.Passengers.AddRange(Group.Customers);
var destination = Group.Destination.Location;
App.Console.Print($"Passengers entered car {car.Id} with destination {destination}", Colors.Blue);
car.Destination = new Destination {
Location = destination, Road = GPSSystem.NearestRoad(destination)
};
car.Controller.PassengersReady();
return;
}
if (buitenRange)
{
return;
}
var road = GPSSystem.NearestRoad(Group.Destination.Location);
GPSSystem.RequestCar(new CarSystem.Destination {
Location = Group.Destination.Location, Road = road
}, Group);
RequestedCar = true;
}
private static Node CalculatePathNextNode(long callerId, ref NodeNetworkCopy network, ref Node startNode, ref Node endNode, Destination endDestination)
{
var openNodes = new List <ConnectedNode> {
new ConnectedNode(endNode)
};
var closedNodes = new List <ConnectedNode>();
ConnectedNode lastNode = null;
var updateIndex = 0;
while (true)
{
// propogate
var thisNode = openNodes.First();
openNodes.Remove(thisNode);
closedNodes.Add(thisNode);
var neighbours = network.Links
.Where(l => l.NodeA.Equals(thisNode) || l.NodeB.Equals(thisNode))
.Select(l => l.NodeA.Equals(thisNode) ? new ConnectedNode(l.NodeB) : new ConnectedNode(l.NodeA))
.ToList();
// calculate
var foundEnd = false;
foreach (var n in neighbours)
{
if (closedNodes.SingleOrDefault(cn => cn.Equals(n)) != null)
{
continue;
}
if (n.Equals(startNode))
{
foundEnd = true;
lastNode = n;
}
n.ConnectedTo = thisNode;
n.Value = Math.Sqrt(Math.Pow((n.PositionX - startNode.PositionX), 2) + Math.Pow((n.PositionY - startNode.PositionY), 2));
network.Nodes.Single(nn => nn.Equals(n)).Value = n.Value;
#if DEBUG
AlgorithmDebuggerWindow.Instance.AddNodeUpdateResult($"AS{_debuggerIndex}${updateIndex++}", network,
n, thisNode);
#endif
openNodes.Add(n);
}
if (foundEnd)
{
break;
}
// sort
openNodes = openNodes.OrderBy(n => n.Value).ToList();
// info
#if DEBUG
AlgorithmDebuggerWindow.Instance.AddNetworkResult($"AS{_debuggerIndex}#{openNodes.First().Value}", network,
thisNode, openNodes.First());
#endif
}
var roadX = (startNode.PositionX - lastNode.ConnectedTo.PositionX) / 2.0 + lastNode.ConnectedTo.PositionX;
var roadY = (startNode.PositionY - lastNode.ConnectedTo.PositionY) / 2.0 + lastNode.ConnectedTo.PositionY;
var result = new Destination
{
Location = new Vector(lastNode.ConnectedTo.PositionX, lastNode.ConnectedTo.PositionY),
Road = GPSSystem.NearestRoad(new Vector(roadX, roadY))
};
if (_calculationCache.ContainsKey(callerId))
{
_calculationCache.Remove(callerId);
}
_calculationCache.Add(callerId, new Tuple <Destination, List <ConnectedNode> >(endDestination, closedNodes));
// find next node
return(lastNode.ConnectedTo);
}
public Car SpawnCar(int id, Destination finalDestination)
{
if (!hasDirection)
{
var nearest = GPSSystem.NearestRoad(Location);
Direction = nearest.IsXRoad() ? DirectionCar.East : DirectionCar.North;
hasDirection = true;
}
if (AvailableCars <= 0)
{
return(null);
}
foreach (var cityCar in City.Instance.Cars)
{
if (MathUtil.Distance(Location, cityCar.Location) < 100)
{
var thread = new Thread(() =>
{
Thread.Sleep(1000);
SpawnCar(id, finalDestination);
});
thread.Start();
return(null);
}
}
var road = GPS.GPSSystem.NearestRoad(Location);
double x;
double y;
if (road.IsXRoad())
{
x = Location.X;
y = Direction == DirectionCar.East ? road.Start.Y + road.Width / 3d : road.Start.Y - road.Width / 3d;
}
else
{
y = Location.Y;
x = Direction == DirectionCar.South ? road.Start.X - road.Width / 3d : road.Start.X + road.Width / 3d;
}
var location = new Vector(x, y);
var car = Model.CreateCar(id, location, this, Direction);
Vector destination;
switch (Direction)
{
case DirectionCar.North:
destination = new Vector(road.Start.X, Math.Min(road.Start.Y, road.End.Y));
break;
case DirectionCar.South:
destination = new Vector(road.Start.X, Math.Max(road.Start.Y, road.End.Y));
break;
case DirectionCar.East:
destination = new Vector(Math.Max(road.Start.X, road.End.X), road.Start.Y);
break;
case DirectionCar.West:
destination = new Vector(Math.Min(road.Start.X, road.End.X), road.Start.Y);
break;
default:
throw new ArgumentException("Unknown direction");
}
car.Destination = new Destination
{
Location = destination,
Road = road
};
car.CurrentTarget = destination;
City.Instance.Cars.Add(car);
AvailableCars--;
car.Destination = finalDestination;
return(car);
}
/// <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;
}