private ReverseResumableAStar getRRA(Cell startCell, Cell destinationCell)
{
// if the RRA destination cell is too far from the current cell (8 cells), then it is restarted
if (rraDict.ContainsKey(destinationCell))
{
var rra = rraDict[destinationCell];
if (rra.HasCell(startCell) || Cell.ManhanttanDistance(rra.DestinationCell, startCell) <= 8)
{
return(rra);
}
}
var newRRA = new ReverseResumableAStar(destinationCell, startCell);
rraDict[destinationCell] = newRRA;
return(newRRA);
}
/// <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>
/// Initializes a new instance of the <see cref="SpaceTimeAStar"/> class.
/// </summary>
public SpaceTimeAStarSteper(Graph graph, double lengthOfAWaitStep, Agent agent, ReverseResumableAStar rraStar)
: base(graph, lengthOfAWaitStep, 0, null, agent, rraStar, false)
{
//we don't need the AStar Base things
Q = null;
Open = null;
Closed = null;
}
/// <summary>
/// Initializes a new instance of the <see cref="PASActionGenerator"/> class.
/// </summary>
/// <param name="graph">The graph.</param>
/// <param name="lengthOfAWaitStep">The length of a wait step.</param>
/// <param name="agent">The agent.</param>
/// <param name="obstacles">The obstacles.</param>
/// <param name="rraStar">The rra star.</param>
public PASActionGenerator(Graph graph, double lengthOfAWaitStep, Agent agent, ReverseResumableAStar rraStar)
: base(graph, lengthOfAWaitStep, double.PositiveInfinity, null, agent, rraStar)
{
}
/// <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);
}
}
}
}
