/// <summary>
/// Solves the specified node.
/// </summary>
/// <param name="node">The node.</param>
/// <param name="currentTime">The current time.</param>
/// <param name="agent">The agent.</param>
/// <param name="obstacleNodes">The obstacle nodes.</param>
/// <param name="lockedNodes">The locked nodes.</param>
/// <returns></returns>
private bool Solve(ConflictTree.Node node, double currentTime, Agent agent)
{
//clear reservation table
_reservationTable.Clear();
//add constraints (agentId = -1 => Intervals from the tree)
foreach (var constraint in node.getConstraints(agent.ID))
{
_reservationTable.Add(constraint.IntervalConstraint);
}
//drive to next node must be possible - otherwise it is not a valid node
foreach (var reservation in agent.ReservationsToNextNode)
{
if (!_reservationTable.IntersectionFree(reservation))
{
return(false);
}
}
//We can use WHCA Star here in a low level approach.
//Window = Infinitively long
var rraStar = new ReverseResumableAStar(Graph, agent, agent.Physics, agent.DestinationNode);
var aStar = new SpaceTimeAStar(Graph, LengthOfAWaitStep, double.PositiveInfinity, _reservationTable, agent, rraStar);
//execute
var found = aStar.Search();
//+ WHCA* Nodes
List <ReservationTable.Interval> reservations;
Path path = new Path();
if (found)
{
//add all WHCA Nodes
aStar.GetPathAndReservations(ref path, out reservations);
#if DEBUG
foreach (var reservation in reservations)
{
Debug.Assert(_reservationTable.IntersectionFree(reservation));
}
#endif
//add the next node again
if (path.Count == 0 || path.NextAction.Node != agent.NextNode || path.NextAction.StopAtNode == false)
{
path.AddFirst(agent.NextNode, true, 0);
}
node.setSolution(agent.ID, path, reservations);
//found
return(true);
}
else
{
//not found
return(false);
}
}
/// <summary>
/// Find the path for all the agents.
/// </summary>
/// <param name="agents">agents</param>
/// <param name="queues">The queues for the destination, starting with the destination point.</param>
/// <param name="nextReoptimization">The next re-optimization time.</param>
/// <returns>
/// paths
/// </returns>
/// <exception cref="System.Exception">Here I have to do something</exception>
public override void FindPaths(double currentTime, List <Agent> agents)
{
Stopwatch.Restart();
//Reservation Table
_reservationTable.Reorganize(currentTime);
//remove all agents that are not affected by this search
foreach (var missingAgentId in _calculatedReservations.Keys.Where(id => !agents.Any(a => a.ID == id)))
{
_reservationTable.Remove(_calculatedReservations[missingAgentId]);
}
//sort Agents
agents = agents.OrderBy(a => a.CanGoThroughObstacles ? 1 : 0).ThenBy(a => Graph.getDistance(a.NextNode, a.DestinationNode)).ToList();
Dictionary <int, double> bias = new Dictionary <int, double>();
//deadlock handling
if (UseDeadlockHandler)
{
_deadlockHandler.LengthOfAWaitStep = LengthOfAWaitStep;
_deadlockHandler.MaximumWaitTime = 30;
_deadlockHandler.Update(agents, currentTime);
}
//optimize Path
if (UseBias)
{
foreach (var agent in agents.Where(a => a.RequestReoptimization && !a.FixedPosition && a.NextNode != a.DestinationNode))
{
//Create RRA* search if necessary.
//Necessary if the agent has none or the agents destination has changed
ReverseResumableAStar rraStar;
if (!rraStars.TryGetValue(agent.ID, out rraStar) || rraStar == null || rraStar.StartNode != agent.DestinationNode ||
UseDeadlockHandler && _deadlockHandler.IsInDeadlock(agent, currentTime)) // TODO this last expression is used to set back the state of the RRA* in case of a deadlock - this is only a hotfix
{
rraStars[agent.ID] = new ReverseResumableAStar(Graph, agent, agent.Physics, agent.DestinationNode);
}
if (rraStars[agent.ID].Closed.Contains(agent.NextNode) || rraStars[agent.ID].Search(agent.NextNode))
{
var nodes = rraStars[agent.ID].getPathAsNodeList(agent.NextNode);
nodes.Add(agent.NextNode);
foreach (var node in nodes)
{
if (!bias.ContainsKey(node))
{
bias.Add(node, 0.0);
}
bias[node] += LengthOfAWaitStep * 1.0001;
}
}
}
}
//optimize Path
foreach (var agent in agents.Where(a => a.RequestReoptimization && !a.FixedPosition && a.NextNode != a.DestinationNode))
{
//runtime exceeded
if (Stopwatch.ElapsedMilliseconds / 1000.0 > RuntimeLimitPerAgent * agents.Count * 0.9 || Stopwatch.ElapsedMilliseconds / 1000.0 > RunTimeLimitOverall)
{
Communicator.SignalTimeout();
return;
}
//remove existing reservations of previous calculations that are not a victim of the reorganization
_reservationTable.Remove(_calculatedReservations[agent.ID]);
//all reservations deleted
_calculatedReservations[agent.ID].Clear();
if (!UseBias)
{
//Create RRA* search if necessary.
//Necessary if the agent has none or the agents destination has changed
ReverseResumableAStar rraStar;
if (!rraStars.TryGetValue(agent.ID, out rraStar) || rraStar == null || rraStar.StartNode != agent.DestinationNode ||
UseDeadlockHandler && _deadlockHandler.IsInDeadlock(agent, currentTime)) // TODO this last expression is used to set back the state of the RRA* in case of a deadlock - this is only a hotfix
{
rraStars[agent.ID] = new ReverseResumableAStar(Graph, agent, agent.Physics, agent.DestinationNode);
}
}
//search my path to the goal
var aStar = new SpaceTimeAStar(Graph, LengthOfAWaitStep, currentTime + LengthOfAWindow, _reservationTable, agent, rraStars[agent.ID]);
aStar.FinalReservation = true;
List <int> nodes = null;
if (UseBias)
{
aStar.BiasedCost = bias;
if (rraStars[agent.ID].Closed.Contains(agent.NextNode) || rraStars[agent.ID].Search(agent.NextNode))
{
//subtract my own cost
nodes = rraStars[agent.ID].getPathAsNodeList(agent.NextNode);
nodes.Add(agent.NextNode);
foreach (var node in nodes)
{
bias[node] -= LengthOfAWaitStep * 1.0001;
}
}
}
//execute
var found = aStar.Search();
if (UseBias && nodes != null)
{
//re add my own cost
foreach (var node in nodes)
{
bias[node] += LengthOfAWaitStep * 1.0001;
}
}
if (!found)
{
//set a fresh reservation for my current node
_reservationTable.Clear(agent.NextNode);
_calculatedReservations[agent.ID] = new List <ReservationTable.Interval>(new ReservationTable.Interval[] { new ReservationTable.Interval(agent.NextNode, 0, double.PositiveInfinity) });
_reservationTable.Add(_calculatedReservations[agent.ID]);
agent.Path = new Path();
//clear all reservations of other agents => they will not calculate a path over this node anymore
foreach (var otherAgent in _calculatedReservations.Keys.Where(id => id != agent.ID))
{
_calculatedReservations[otherAgent].RemoveAll(r => r.Node == agent.NextNode);
}
//add wait step
agent.Path.AddFirst(agent.NextNode, true, LengthOfAWaitStep);
//add the next node again
if (agent.ReservationsToNextNode.Count > 0 && (agent.Path.Count == 0 || agent.Path.NextAction.Node != agent.NextNode || agent.Path.NextAction.StopAtNode == false))
{
agent.Path.AddFirst(agent.NextNode, true, 0);
}
continue;
}
//just wait where you are and until the search time reaches currentTime + LenghtOfAWindow is always a solution
Debug.Assert(found);
//+ WHCA* Nodes
agent.Path = new Path();
List <ReservationTable.Interval> reservations;
aStar.GetPathAndReservations(ref agent.Path, out reservations);
_calculatedReservations[agent.ID] = reservations;
//add to reservation table
_reservationTable.Add(_calculatedReservations[agent.ID]);
//add reservation to infinity
var lastNode = (_calculatedReservations[agent.ID].Count > 0) ? _calculatedReservations[agent.ID][_calculatedReservations[agent.ID].Count - 1].Node : agent.NextNode;
var lastTime = (_calculatedReservations[agent.ID].Count > 0) ? _calculatedReservations[agent.ID][_calculatedReservations[agent.ID].Count - 1].End : currentTime;
_calculatedReservations[agent.ID].Add(new ReservationTable.Interval(lastNode, lastTime, double.PositiveInfinity));
try
{
_reservationTable.Add(lastNode, lastTime, double.PositiveInfinity);
}
catch (DisjointIntervalTree.IntervalIntersectionException)
{
//This could technically fail => especially when they come from a station
}
//add the next node again
if (agent.ReservationsToNextNode.Count > 0 && (agent.Path.Count == 0 || agent.Path.NextAction.Node != agent.NextNode || agent.Path.NextAction.StopAtNode == false))
{
agent.Path.AddFirst(agent.NextNode, true, 0);
}
//next time ready?
if (agent.Path.Count == 0)
{
rraStars[agent.ID] = null;
}
//deadlock?
if (UseDeadlockHandler)
{
if (_deadlockHandler.IsInDeadlock(agent, currentTime))
{
_deadlockHandler.RandomHop(agent);
}
}
}
}
/// <summary>
/// Find the path for all the agents.
/// </summary>
/// <param name="agents">agents</param>
/// <param name="obstacleWaypoints">The way points of the obstacles.</param>
/// <param name="queues">The queues for the destination, starting with the destination point.</param>
/// <param name="nextReoptimization">The next re-optimization time.</param>
/// <returns>
/// paths
/// </returns>
/// <exception cref="System.Exception">Here I have to do something</exception>
private bool _findPaths(double currentTime, List <Agent> agents, Dictionary <int, int> agentPrios, bool lastRun, double runtimeLimit)
{
var conflictFree = true;
//Reservation Table
_reservationTable.Clear();
var fixedBlockage = AgentInfoExtractor.getStartBlockage(agents, currentTime);
SortAgents(ref agents, agentPrios);
//set fixed blockage
foreach (var interval in fixedBlockage.Values.SelectMany(d => d))
{
_reservationTable.Add(interval);
}
//deadlock handling
if (UseDeadlockHandler)
{
_deadlockHandler.LengthOfAWaitStep = LengthOfAWaitStep;
_deadlockHandler.MaximumWaitTime = 30;
_deadlockHandler.Update(agents, currentTime);
}
//optimize Path
foreach (var agent in agents.Where(a => !a.FixedPosition))
{
if (Stopwatch.ElapsedMilliseconds / 1000.0 > runtimeLimit * 0.9)
{
Communicator.SignalTimeout();
return(true);
}
//Create RRA* search if necessary.
//Necessary if the agent has none or the agents destination has changed
ReverseResumableAStar rraStar;
if (!rraStars.TryGetValue(agent.ID, out rraStar) || rraStar.StartNode != agent.DestinationNode ||
UseDeadlockHandler && _deadlockHandler.IsInDeadlock(agent, currentTime)) // TODO this last expression is used to set back the state of the RRA* in case of a deadlock - this is only a hotfix
{
rraStars[agent.ID] = new ReverseResumableAStar(Graph, agent, agent.Physics, agent.DestinationNode);
}
//search my path to the goal
var aStar = new SpaceTimeAStar(Graph, LengthOfAWaitStep, currentTime + LengthOfAWindow, _reservationTable, agent, rraStars[agent.ID]);
//the agent with a higher priority has to wait so that the others can go out of the way
aStar.WaitStepsBeforeStart = (int)(Math.Pow(2, agentPrios[agent.ID]) / 2.0);
//execute
var found = aStar.Search();
if (!found)
{
conflictFree = false;
//fall back => ignore the collisions
agentPrios[agent.ID]++;
if (!lastRun)
{
if (!AbortAtFirstConflict)
{
continue;
}
else
{
return(false);
}
}
}
//+ WHCA* Nodes
List <ReservationTable.Interval> reservations;
if (found)
{
aStar.GetPathAndReservations(ref agent.Path, out reservations);
foreach (var reservation in reservations)
{
_reservationTable.Add(reservation);
}
}
//+ RRA* Nodes
if (aStar.GoalNode >= 0)
{
rraStars[agent.ID].addPath(agent.Path, aStar.NodeTo2D(aStar.GoalNode));
}
//add the next node again
if (fixedBlockage.Count > 0 && (agent.Path.Count == 0 || agent.Path.NextAction.Node != agent.NextNode || agent.Path.NextAction.StopAtNode == false))
{
agent.Path.AddFirst(agent.NextNode, true, 0);
}
//next time ready?
if (agent.Path.Count == 0)
{
rraStars[agent.ID] = null;
}
//deadlock?
if (UseDeadlockHandler)
{
if (_deadlockHandler.IsInDeadlock(agent, currentTime))
{
_deadlockHandler.RandomHop(agent);
}
}
}
return(conflictFree);
}