public override void FindPaths(double currentTime, List <Agent> agents)
{
//randomHop
_deadlockHandler.LengthOfAWaitStep = LengthOfAWaitStep;
foreach (var agent in agents.Where(a => !a.FixedPosition))
{
_deadlockHandler.RandomHop(agent);
}
}
/// <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="currentTime">The current Time.</param>
/// <param name="agents">agents</param>
public override void FindPaths(double currentTime, List <Agent> agents)
{
Stopwatch.Restart();
//initialization data structures
var conflictTree = new ConflictTree();
var Open = new FibonacciHeap <double, ConflictTree.Node>();
var solvable = true;
var generatedNodes = 0;
ConflictTree.Node bestNode = null;
double bestTime = 0.0;
//deadlock handling
_deadlockHandler.LengthOfAWaitStep = LengthOfAWaitStep;
_deadlockHandler.MaximumWaitTime = 30;
_deadlockHandler.Update(agents, currentTime);
//simply blocked
foreach (var agent in agents.Where(a => a.FixedPosition))
{
Graph.NodeInfo[agent.NextNode].IsLocked = true;
}
// TODO this only works as long as a possible solution is guaranteed - maybe instead ignore paths to plan for agents with no possible solution and hope that it clears by others moving on?
//first node initialization
List <Agent> unsolvableAgents = null;
foreach (var agent in agents.Where(a => !a.FixedPosition))
{
bool agentSolved = Solve(conflictTree.Root, currentTime, agent);
if (!agentSolved)
{
if (unsolvableAgents == null)
{
unsolvableAgents = new List <Agent>()
{
agent
}
}
;
else
{
unsolvableAgents.Add(agent);
}
}
solvable = solvable && agentSolved;
}
//node selection strategy (Queue will pick the node with minimum value
NodeSelectionExpression nodeObjectiveSelector = node =>
{
switch (SearchMethod)
{
case CBSSearchMethod.BestFirst:
return(node.SolutionCost);
case CBSSearchMethod.BreathFirst:
return(node.Depth);
case CBSSearchMethod.DepthFirst:
return((-1) * node.Depth);
default:
return(0);
}
};
//Enqueue first node
if (solvable)
{
Open.Enqueue(conflictTree.Root.SolutionCost, conflictTree.Root);
}
else
{
Communicator.LogDefault("WARNING! Aborting CBS - could not obtain an initial solution for the following agents: " +
string.Join(",", unsolvableAgents.Select(a => "Agent" + a.ID.ToString() + "(" + a.NextNode.ToString() + "->" + a.DestinationNode.ToString() + ")")));
}
bestNode = conflictTree.Root;
//search loop
ConflictTree.Node p = conflictTree.Root;
while (Open.Count > 0)
{
//local variables
int agentId1;
int agentId2;
ReservationTable.Interval interval;
//pop out best node
p = Open.Dequeue().Value;
//check the path
var hasNoConflicts = ValidatePath(p, agents, out agentId1, out agentId2, out interval);
//has no conflicts?
if (hasNoConflicts)
{
bestNode = p;
break;
}
// time up? => return the best solution
if (Stopwatch.ElapsedMilliseconds / 1000.0 > RuntimeLimitPerAgent * agents.Count * 0.9 || Stopwatch.ElapsedMilliseconds / 1000.0 > RunTimeLimitOverall)
{
Communicator.SignalTimeout();
break;
}
//save best node
if (bestNode == null || interval.Start > bestTime)
{
bestTime = interval.Start;
bestNode = p;
}
//append child 1
var node1 = new ConflictTree.Node(agentId1, interval, p);
solvable = Solve(node1, currentTime, agents.First(a => a.ID == agentId1));
if (solvable)
{
Open.Enqueue(node1.SolutionCost, node1);
}
//append child 2
var node2 = new ConflictTree.Node(agentId2, interval, p);
solvable = Solve(node2, currentTime, agents.First(a => a.ID == agentId2));
if (solvable)
{
Open.Enqueue(node2.SolutionCost, node2);
}
generatedNodes += 2;
}
//return the solution => suboptimal
foreach (var agent in agents)
{
agent.Path = bestNode.getSolution(agent.ID);
if (_deadlockHandler.IsInDeadlock(agent, currentTime))
{
_deadlockHandler.RandomHop(agent);
}
}
}
public override void FindPaths(double currentTime, List <Agent> agents)
{
Stopwatch.Restart();
var fixedBlockage = AgentInfoExtractor.getStartBlockage(agents, currentTime);
//deadlock handling
_deadlockHandler.LengthOfAWaitStep = LengthOfAWaitStep;
_deadlockHandler.MaximumWaitTime = 30;
_deadlockHandler.Update(agents, currentTime);
//initiate biased costs
_biasedCost.Clear();
foreach (var agent in agents)
{
_biasedCost.Add(agent.ID, new Dictionary <int, double>());
}
while (true)
{
//clear reservation table
_reservationTable.Clear();
//set fixed blockage
try
{
foreach (var agent in agents)
{
//all intervals from now to the moment of stop
foreach (var interval in fixedBlockage[agent])
{
_reservationTable.Add(interval, agent.ID);
}
//reservation of next node
_reservationTable.Add(agent.NextNode, agent.ArrivalTimeAtNextNode, double.PositiveInfinity, agent.ID);
}
}
catch (RAWSimO.MultiAgentPathFinding.DataStructures.DisjointIntervalTree.IntervalIntersectionException) { }
var collisionFound = false;
foreach (var agent in agents.Where(a => !a.FixedPosition && a.NextNode != a.DestinationNode))
{
if (Stopwatch.ElapsedMilliseconds / 1000.0 > RuntimeLimitPerAgent * agents.Count * 0.9 || Stopwatch.ElapsedMilliseconds / 1000.0 > RunTimeLimitOverall)
{
Communicator.SignalTimeout();
return;
}
//remove blocking next node
_reservationTable.RemoveIntersectionWithTime(agent.NextNode, agent.ArrivalTimeAtNextNode);
if (_deadlockHandler.IsInDeadlock(agent, currentTime))
{
_deadlockHandler.RandomHop(agent, _reservationTable, currentTime, true, true);
continue;
}
//Create A* search if necessary.
//Necessary if the agent has none or the agents destination has changed
var aStar = new SpaceAStar(Graph, agent.NextNode, agent.DestinationNode, agent.OrientationAtNextNode, agent.Physics, agent, _biasedCost[agent.ID]);
var found = aStar.Search();
//the search ended with no result => just wait a moment
if (!found)
{
//Can not find a path. Maybe the agent has to wait until the blocked nodes moved
waitStep(agent);
continue;
}
//get the path
List <ReservationTable.Interval> reservations;
agent.Path.Clear();
aStar.getReservationsAndPath(agent.ArrivalTimeAtNextNode, ref agent.Path, out reservations);
reservations.Last().End = double.PositiveInfinity;
//check whether the agent collides with an other agent on its way
int collidesWithAgent;
int collidesOnNode;
if (!_reservationTable.IntersectionFree(reservations, out collidesOnNode, out collidesWithAgent))
{
agent.Path.SetStopBeforeNode(collidesOnNode);
//collision => add biased cost for the current agent
//the other agent remains with the old cost. He has the higher priority
if (_biasedCost[agent.ID].ContainsKey(collidesOnNode))
{
_biasedCost[agent.ID][collidesOnNode] += BiasedCostAmount;
}
else
{
_biasedCost[agent.ID][collidesOnNode] = BiasedCostAmount;
}
collisionFound = true;
}
else
{
_reservationTable.Add(reservations, agent.ID);
}
}
if (!collisionFound)
{
return;
}
}
}
/// <summary>
/// Find the path for all the agents.
/// </summary>
/// <param name="currentTime">The current time.</param>
/// <param name="agents">agents</param>
/// <param name="obstacleNodes">The way points of the obstacles.</param>
/// <param name="lockedNodes">The locked nodes.</param>
/// <param name="nextReoptimization">The next re-optimization time.</param>
/// <param name="runtimeLimit">The runtime limit.</param>
public override void FindPaths(double currentTime, List <Agent> agents)
{
Stopwatch.Restart();
//Reservation Table
_reservationTable.Clear();
var fixedBlockage = AgentInfoExtractor.getStartBlockage(agents, currentTime);
//panic times initialization
foreach (var agent in agents)
{
if (!_waitTime.ContainsKey(agent.ID))
{
_waitTime.Add(agent.ID, double.NegativeInfinity);
}
if (!_moveTime.ContainsKey(agent.ID))
{
_moveTime.Add(agent.ID, currentTime);
}
if (!_es2evadedFrom.ContainsKey(agent.ID))
{
_es2evadedFrom.Add(agent.ID, new HashSet <int>());
}
}
//set fixed blockage
try
{
foreach (var agent in agents)
{
//all intervals from now to the moment of stop
foreach (var interval in fixedBlockage[agent])
{
_reservationTable.Add(interval, agent.ID);
}
//reservation of next node
int collisionAgent;
if (_reservationTable.IntersectionFree(agent.NextNode, agent.ArrivalTimeAtNextNode, double.PositiveInfinity, out collisionAgent))
{
_reservationTable.Add(agent.NextNode, agent.ArrivalTimeAtNextNode, double.PositiveInfinity, agent.ID);
}
else
{
Debug.Assert(collisionAgent == agent.ID);
}
}
}
catch (RAWSimO.MultiAgentPathFinding.DataStructures.DisjointIntervalTree.IntervalIntersectionException) { }
//deadlock handling
if (UseDeadlockHandler)
{
_deadlockHandler.LengthOfAWaitStep = LengthOfAWaitStep;
_deadlockHandler.MaximumWaitTime = 30;
_deadlockHandler.Update(agents, currentTime);
}
//optimize Path
bool deadlockBreakingManeuver;
var reservationOwnerAgentId = -1;
int reservationOwnerNodeId = -1;
List <int> nextHopNodes;
foreach (Agent a in agents)
{
if (a.Path != null && !a.Path.IsConsistent)
{
throw new Exception("fs ex");
}
}
foreach (var agent in agents.Where(a => !a.FixedPosition && a.NextNode != a.DestinationNode && a.ArrivalTimeAtNextNode <= currentTime && a.RequestReoptimization))
{
//runtime exceeded
if (Stopwatch.ElapsedMilliseconds / 1000.0 > RuntimeLimitPerAgent * agents.Count * 0.9 || Stopwatch.ElapsedMilliseconds / 1000.0 > RunTimeLimitOverall)
{
Communicator.SignalTimeout();
return;
}
deadlockBreakingManeuver = false;
//agent is allowed to move?
if (currentTime < _waitTime[agent.ID])
{
continue;
}
//remove blocking next node
_reservationTable.RemoveIntersectionWithTime(agent.NextNode, agent.ArrivalTimeAtNextNode);
//Create RRA* search if necessary.
//Necessary if the agent has none or the agents destination has changed
ReverseResumableAStar rraStar;
if (!_rraStar.TryGetValue(agent.ID, out rraStar) || rraStar == null || rraStar.StartNode != agent.DestinationNode)
{
//new search
rraStar = new ReverseResumableAStar(Graph, agent, agent.Physics, agent.DestinationNode, new HashSet <int>());
_rraStar[agent.ID] = rraStar;
_moveTime[agent.ID] = currentTime;
}
//already found in RRA*?
var found = rraStar.Closed.Contains(agent.NextNode);
//If the search is old, the path may me blocked now
if (found && rraStar.PathContains(agent.NextNode))
{
//new search
rraStar = new ReverseResumableAStar(Graph, agent, agent.Physics, agent.DestinationNode, new HashSet <int>());
_rraStar[agent.ID] = rraStar;
found = false;
}
//Is a search processing necessary
if (!found)
{
found = rraStar.Search(agent.NextNode);
}
//the search ended with no result => just wait a moment
if (!found)
{
//new search
_rraStar[agent.ID] = null;
//still not found? Then wait!
if (_waitTime[agent.ID] < currentTime - LengthOfAWaitStep * 2f)
{
waitStep(agent, agent.ID, currentTime);
continue;
}
else
{
deadlockBreakingManeuver = true;
}
}
if (!deadlockBreakingManeuver)
{
//set the next step of the path
nextHopNodes = _getNextHopNodes(currentTime, agent, out reservationOwnerAgentId, out reservationOwnerNodeId);
//avoid going back
if (Es2BackEvadingAvoidance && nextHopNodes.Count > 1 && _es2evadedFrom[agent.ID].Contains(nextHopNodes[1]))
{
deadlockBreakingManeuver = true;
}
else if (Es2BackEvadingAvoidance)
{
_es2evadedFrom[agent.ID].Clear();
}
//found a path => set it
if (!deadlockBreakingManeuver && nextHopNodes.Count > 1)
{
_setPath(nextHopNodes, agent);
_moveTime[agent.ID] = currentTime;
continue;
}
}
//deadlock breaking maneuver due to wait time
if (!deadlockBreakingManeuver)
{
deadlockBreakingManeuver = currentTime - _moveTime[agent.ID] > MaximumWaitTime;
}
//deadlock breaking maneuver due to wait for relation circle
if (!deadlockBreakingManeuver)
{
//transitive closure of wait for relation
HashSet <int> waitForSet = new HashSet <int>();
var waitForID = agent.ID;
while (!deadlockBreakingManeuver && _waitFor.ContainsKey(waitForID))
{
if (waitForSet.Contains(waitForID))
{
deadlockBreakingManeuver = true;
}
waitForSet.Add(waitForID);
waitForID = _waitFor[waitForID];
}
}
if (!deadlockBreakingManeuver)
{
//wait a little while
waitStep(agent, reservationOwnerAgentId, currentTime);
continue;
}
else
{
//deadlock breaking maneuver must be done!
switch (_evadingStragety)
{
case EvadingStrategy.EvadeByRerouting:
//obstacle free
if (!found)
{
rraStar = _rraStar[agent.ID] = new ReverseResumableAStar(Graph, agent, agent.Physics, agent.DestinationNode, new HashSet <int>());
}
for (int tries = 1; tries <= Es1MaximumNumberOfBreakingManeuverTries; tries++)
{
//deadlock breaking maneuver must be done!
if (tries >= Es1MaximumNumberOfBreakingManeuverTries)
{
//wait a little while => for myself
waitStep(agent, agent.ID, currentTime);
break;
}
//The agent can not move
_waitFor[agent.ID] = reservationOwnerAgentId;
//add an obstacle and restart the search
rraStar.AddCustomLock(reservationOwnerNodeId);
rraStar.Clear(agent.DestinationNode, agent.NextNode);
found = rraStar.Search(agent.NextNode);
//found => get hop nodes
if (!found)
{
//wait a little while => for myself
waitStep(agent, agent.ID, currentTime);
break;
}
else
{
nextHopNodes = _getNextHopNodes(currentTime, agent, out reservationOwnerAgentId, out reservationOwnerNodeId);
if (nextHopNodes.Count > 1)
{
_setPath(nextHopNodes, agent);
break;
}
}
}
break;
case EvadingStrategy.EvadeToNextNode:
//try to find a free hop
var foundBreakingManeuverEdge = false;
var possibleEdges = new List <Edge>(Graph.Edges[agent.NextNode]);
shuffle <Edge>(possibleEdges, Randomizer);
foreach (var edge in possibleEdges.Where(e => !e.ToNodeInfo.IsLocked && (agent.CanGoThroughObstacles || !e.ToNodeInfo.IsObstacle) && !_es2evadedFrom[agent.ID].Contains(e.To)))
{
//create intervals
var intervals = _reservationTable.CreateIntervals(currentTime, currentTime, 0, agent.Physics, agent.NextNode, edge.To, true);
reservationOwnerNodeId = -1;
reservationOwnerAgentId = -1;
//check if a reservation is possible
if (_reservationTable.IntersectionFree(intervals, out reservationOwnerNodeId, out reservationOwnerAgentId))
{
foundBreakingManeuverEdge = true;
//avoid going back
if (this.Es2BackEvadingAvoidance)
{
_es2evadedFrom[agent.ID].Add(edge.To);
}
//create a path
agent.Path.Clear();
agent.Path.AddLast(edge.To, true, LengthOfAWaitStep);
break;
}
}
if (!foundBreakingManeuverEdge)
{
//Clear the nodes
_es2evadedFrom[agent.ID].Clear();
//just wait
waitStep(agent, agent.ID, currentTime);
}
break;
}
}
foreach (Agent a in agents)
{
if (a.Path != null && !a.Path.IsConsistent)
{
throw new Exception("fs ex");
}
}
//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);
}