A_Star_WithOD.cs

using System;
using System.Collections.Generic;
using System.IO;

namespace mapf
{
    /// <summary>
    /// A* implementation with Standley's operator decomposition (OD).
    /// See AAAI 2010 paper by Trevor Standley on Cooperative Pathfinding.
    /// </summary>
    public class A_Star_WithOD : A_Star
    {
        protected int expandedFullStates;
        protected int accExpandedFullStates;
        protected int generatedFullStates;
        protected int accGeneratedFullStates;

        public A_Star_WithOD(IHeuristicCalculator<WorldState> heuristic = null, bool mStar = false, bool mStarShuffle = false)
            : base(heuristic, mStar, mStarShuffle) { }

        override protected WorldState CreateSearchRoot(int minDepth = -1, int minCost = -1, MDDNode mddNode = null)
        {
            return new WorldStateWithOD(this.instance.agents, minDepth, minCost, mddNode);
        }

        protected override WorldState CreateSearchNode(WorldState from)
        {
            return new WorldStateWithOD((WorldStateWithOD)from);
        }

        public override string GetName() { return base.GetName() + "+OD"; }

        public override void Setup(ProblemInstance problemInstance, int minDepth, Run runner,
                                   ConflictAvoidanceTable CAT = null,
                                   ISet<CbsConstraint> constraints = null, ISet<CbsConstraint> positiveConstraints = null,
                                   int minCost = -1, int maxCost = int.MaxValue, MDD mdd = null)
        {
            base.Setup(problemInstance, minDepth, runner, CAT, constraints, positiveConstraints,
                       minCost, maxCost, mdd);
            this.expandedFullStates = 0;
            this.generatedFullStates = 0;
        }

        protected bool alreadyExpanded;

        // Problem 59 of 3 agents and 9 obstacles is a good example of why
        // equivalence over different times is difficult to get right
        // with A*+OD (but possible).

        /// <summary>
        /// Expand a given node. This includes:
        /// - Generating all possible children
        /// - Inserting them to OPEN
        /// - Insert the generated nodes to the hashtable of nodes, currently implmented together with the closed list.
        /// </summary>
        public override void Expand(WorldState node)
        {
            if (((WorldStateWithOD)node).agentTurn == 0)
                expandedFullStates++;
            this.alreadyExpanded = false;
            base.Expand(node);
        }

        protected override List<WorldState> ExpandOneAgent(List<WorldState> intermediateNodes, int agentIndex)
        {
            if (this.alreadyExpanded == true)  // Necessary because after expansion, the generated nodes have an incremented agentTurn that once again equals agentIndex
                                               // and because it's possible that a node may have valid children that aren't already in the closed list
                return intermediateNodes; // Do nothing to this agent

            WorldStateWithOD parent = (WorldStateWithOD)intermediateNodes[0];

            if (agentIndex < parent.agentTurn)
                return intermediateNodes; // Do nothing to this agent

            var generated = base.ExpandOneAgent(intermediateNodes, agentIndex);

            int childAgentTurn = ((parent.agentTurn + 1) % (this.instance.agents.Length));
            foreach (var node in generated)
            {
                WorldStateWithOD childNode = (WorldStateWithOD)node;

                childNode.agentTurn = childAgentTurn;

                // Makespan increases only if this is the move of the first agent. This makes sure that under a makespan
                // cost function, partial nodes have a correct cost and can even serve as goal nodes.
                if (parent.agentTurn != 0)
                    childNode.makespan--; // Cancel the increment in base
            }

            this.alreadyExpanded = true;
            return generated;
        }

        protected override bool ProcessGeneratedNode(WorldState currentNode)
        {
            bool ret = base.ProcessGeneratedNode(currentNode);
            var node = (WorldStateWithOD)currentNode;
            if (node.agentTurn == 0)
                this.generatedFullStates++;
            return ret;
        }

        public override void OutputStatisticsHeader(TextWriter output)
        {
            base.OutputStatisticsHeader(output);

            output.Write(this.ToString() + " Expanded Full States");
            output.Write(Run.RESULTS_DELIMITER);
            output.Write(this.ToString() + " Generated Full States");
            output.Write(Run.RESULTS_DELIMITER);
        }

        public override void OutputStatistics(TextWriter output)
        {
            base.OutputStatistics(output);

            Console.WriteLine("Total Expanded Full States: {0}", this.expandedFullStates);
            Console.WriteLine("Total Generated Full States: {0}", this.generatedFullStates);

            output.Write(this.expandedFullStates + Run.RESULTS_DELIMITER);
            output.Write(this.generatedFullStates + Run.RESULTS_DELIMITER);
        }

        public override int NumStatsColumns
        {
            get
            {
                return 2 + base.NumStatsColumns;
            }
        }

        public override void ClearAccumulatedStatistics()
        {
            base.ClearAccumulatedStatistics();

            this.accExpandedFullStates = 0;
            this.accGeneratedFullStates = 0;
        }

        public override void AccumulateStatistics()
        {
            base.AccumulateStatistics();

            this.accExpandedFullStates += this.expandedFullStates;
            this.accGeneratedFullStates += this.generatedFullStates;
        }

        public override void OutputAccumulatedStatistics(TextWriter output)
        {
            base.OutputAccumulatedStatistics(output);

            Console.WriteLine("{0} Average Expanded Full States (Low-Level): {1}", this, this.accExpandedFullStates);
            Console.WriteLine("{0} Average Generated Full States (Low-Level): {1}", this, this.accGeneratedFullStates);

            output.Write(this.accExpandedFullStates + Run.RESULTS_DELIMITER);
            output.Write(this.accGeneratedFullStates + Run.RESULTS_DELIMITER);
        }
    }
}