Пример #1
0
        /// <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;
                }
            }
        }
Пример #2
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);
        }