/// <summary>
/// Generates the wait node.
/// </summary>
/// <param name="parent">The parent.</param>
/// <returns></returns>
public Tuple <int, List <ReservationTable.Interval>, List <Collision> > GenerateWaitNode(int parent, ReservationTable reservationTable)
{
List <Collision> collisions;
reservationTable.IntersectionFree(NodeTo2D(parent), NodeTime[parent], NodeTime[parent] + _lengthOfAWaitStep, out collisions);
//add successor
var waitNode = Tuple.Create(_numNodeId, new List <ReservationTable.Interval>(new[] { new ReservationTable.Interval(NodeTo2D(parent), NodeTime[parent], NodeTime[parent] + _lengthOfAWaitStep) }), collisions);
NodeTime.Add(NodeTime[parent] + _lengthOfAWaitStep);
NodeBackpointerId.Add(parent);
NodeBackpointerLastStopId.Add(_numNodeId);
NodeBackpointerEdge.Add(null);
_numNodeId++;
return(waitNode);
}
/// <summary>
/// Clears this instance.
/// </summary>
internal void Clear()
{
StartNode = 0;
GoalNode = -1;
//built mappings
NodeTime.Clear();
NodeBackpointerId.Clear();
NodeBackpointerLastStopId.Clear();
NodeBackpointerEdge.Clear();
NodeTimeTemp.Clear();
NodeBackpointerIdTemp.Clear();
NodeBackpointerLastTurnIdTemp.Clear();
NodeBackpointerEdgeTemp.Clear();
_numNodeId = 0;
StartAngle = Graph.RadToDegree(_agent.OrientationAtNextNode);
NodeTime.Add(_agent.ArrivalTimeAtNextNode);
NodeBackpointerId.Add(-1);
NodeBackpointerLastStopId.Add(0);
NodeBackpointerEdge.Add(null);
_numNodeId++;
_init = true;
if (_tieBreaking)
{
_tieBreakingTimes = new double[_graph.NodeCount];
for (var i = 0; i < _tieBreakingTimes.Length; i++)
{
_tieBreakingTimes[i] = -1.0;
}
}
}
/// <summary>
/// Gets the actions.
/// </summary>
/// <param name="node">The node.</param>
/// <returns>actions</returns>
public List <Tuple <int, List <ReservationTable.Interval>, List <Collision> > > GetActions(int n, int prio, ReservationTable reservationTable)
{
var successors = new List <Tuple <int, List <ReservationTable.Interval>, List <Collision> > >();
List <int> checkPointNodes = new List <int>();
List <double> checkPointDistances = new List <double>();
List <double> checkPointTimes;
List <Collision> collisions;
//wait successor
reservationTable.IntersectionFree(NodeTo2D(n), NodeTime[n], NodeTime[n] + _lengthOfAWaitStep, out collisions);
if (collisions == null || collisions.All(c => c.priority < prio || c.agentId == _agent.ID))
{
//add successor
successors.Add(Tuple.Create(_numNodeId, new List <ReservationTable.Interval>(new[] { new ReservationTable.Interval(NodeTo2D(n), NodeTime[n], NodeTime[n] + _lengthOfAWaitStep) }), collisions));
NodeTime.Add(NodeTime[n] + _lengthOfAWaitStep);
NodeBackpointerId.Add(n);
NodeBackpointerLastStopId.Add(_numNodeId);
NodeBackpointerEdge.Add(null);
_numNodeId++;
}
//current angle
short lastStopAngleAfterTurn = GetLastStopAngleAfterTurn(n);
//for each direction
foreach (var direction in _graph.Edges[NodeTo2D(n)])
{
//clear temporary data structures
NodeTimeTemp.Clear();
NodeBackpointerIdTemp.Clear();
NodeBackpointerLastTurnIdTemp.Clear();
NodeBackpointerEdgeTemp.Clear();
//initiate checkpoints
checkPointTimes = null;
checkPointNodes.Clear();
checkPointDistances.Clear();
//Our aim is to calculate the distance for a hop.
//A hop is defined as follows: The longest distance between two nodes, where no turn is necessary.
//The end of the hop is "node". In this method we search for the start ("lastTurnNode") and calculate the g as follows:
//g(node) = time needed to get to "lastTurnNode" + time needed to turn + time needed to get to node
//angle i had before my current angle: lastTurnAngle
//my current angle: thisAngle
//the angle i want to go to: direction.Angle
int lastStopId = NodeBackpointerLastStopId[n];
if (direction.Angle != lastStopAngleAfterTurn)
{
//we turn at n
lastStopId = n;
checkPointDistances.Add(0.0);
checkPointNodes.Add(NodeTo2D(n));
}
else
{
//get the intermediate checkpoints
AddCheckPointDistances(lastStopId, n, checkPointNodes, checkPointDistances);
}
//time needed to get to the target orientation
//last stop angle after turn of the last stop <=> last stop angle before turn
short lastStopAngleBeforeTurn = GetLastStopAngleAfterTurn(lastStopId);
double timeToTurn = _agent.Physics.getTimeNeededToTurn(Graph.DegreeToRad(lastStopAngleBeforeTurn), Graph.DegreeToRad(direction.Angle));
//check if there is enough free time to rotate in the direction
/*
* if (timeToTurn > 0)
* {
* reservationTable.IntersectionFree(NodeTo2D(lastStopId), NodeTime[lastStopId], NodeTime[lastStopId] + timeToTurn, out collisions);
* if (collisions != null && collisions.All(c => c.priority >= prio && c.agentId != _agent.ID))
* continue;
* }
*
* => Do it later
*
*/
//generate successors
var currentNode = NodeTo2D(n);
var agentAngle = direction.Angle;
var backpointerNode = n;
//get drive distance
var driveDistance = checkPointDistances[checkPointDistances.Count - 1];
var foundNext = true;
var pathFree = false;
//try to find the first node in this direction, which path is free
while (foundNext && !pathFree)
{
foundNext = false;
//search for next edges in same direction
foreach (var edge in _graph.Edges[currentNode])
{
if (Math.Abs(edge.Angle - agentAngle) < 2)
{
//skip blocked edges
if (edge.ToNodeInfo.IsLocked || (!_agent.CanGoThroughObstacles && edge.ToNodeInfo.IsObstacle))
{
//blocked
foundNext = false;
break;
}
//cumulate
driveDistance += edge.Distance;
//add checkpoint
checkPointDistances.Add(driveDistance);
checkPointNodes.Add(edge.To);
//check whether it is intersection free
var timeToMove = _agent.Physics.getTimeNeededToMove(0f, NodeTime[lastStopId] + timeToTurn, driveDistance, checkPointDistances, out checkPointTimes);
//add time needed to turn
if (checkPointTimes.Count > 0)
{
checkPointTimes[0] -= timeToTurn;
}
//check if driving action is collision free
reservationTable.IntersectionFree(checkPointNodes, checkPointTimes, false, out collisions);
pathFree = collisions == null || collisions.All(c => c.priority < prio || c.agentId == _agent.ID);
//add node to temp => will be added, if a valid successor will be found
NodeTimeTemp.Add(NodeTime[lastStopId] + timeToTurn + timeToMove);
NodeBackpointerIdTemp.Add(backpointerNode);
NodeBackpointerLastTurnIdTemp.Add(lastStopId);
NodeBackpointerEdgeTemp.Add(edge);
if (pathFree)
{
//treat only the last one as successor
successors.Add(Tuple.Create(_numNodeId + NodeTimeTemp.Count - 1, reservationTable.CreateIntervals(NodeTime[lastStopId], checkPointNodes, checkPointTimes, false), collisions));
_numNodeId += NodeTimeTemp.Count;
//add temporary successors
NodeTime.AddRange(NodeTimeTemp);
NodeBackpointerId.AddRange(NodeBackpointerIdTemp);
NodeBackpointerLastStopId.AddRange(NodeBackpointerLastTurnIdTemp);
NodeBackpointerEdge.AddRange(NodeBackpointerEdgeTemp);
}
backpointerNode = _numNodeId + NodeTimeTemp.Count - 1;
currentNode = edge.To;
foundNext = true;
break;
}
}
}
}
return(successors);
}