public virtual IEnumerable<MoveToAction> Search( Problem problem )
{
List<Node> frontier = new List<Node>();
HashSet<Node> explored = new HashSet<Node>();
Node root = new Node( problem.InitialState );
frontier.Add( root );
while( frontier.Count > 0 ) {
Node nodeToExpand = frontier.First();
frontier.Remove( nodeToExpand );
//Console.WriteLine( nodeToExpand.State );
var newNodes = ExpandNode(
nodeToExpand,
explored,
problem
);
foreach( Node fn in newNodes ) {
var test = fn.Equals( newNodes.First() );
if( IsGoalState( fn.State, problem.GoalTest ) ) {
return ActionsFromNodes( fn.GetPathFromRoot() );
}
frontier.Add( fn );
}
// This could be costly. Find better alternative later
frontier = frontier
.OrderByDescending( n => n.PathCost + n.EstimateCost )
.Distinct()
.ToList();
}
return new List<MoveToAction>();
}
private IEnumerable<Node> ExpandNode(
Node node,
HashSet<Node> explored,
Problem problem
)
{
explored.Add( node );
var childNodes = new List<Node>();
var actionsFunction = problem.ActionsFunction;
var resultFunction = problem.ResultFunction;
var stepCostFunction = problem.StepCostFunction;
foreach( var action in actionsFunction.GetActions( node.State ) ) {
string successorState = resultFunction.Result(
node.State,
action
);
int stepCost = stepCostFunction.Cost(
node.State,
action,
successorState
);
int estimateCost = m_heuristic.Calculate( successorState );
var child = new Node(
successorState,
node,
action,
stepCost,
estimateCost
);
if( !explored.Contains( child ) ) {
childNodes.Add( child );
}
}
return childNodes;
}
public AStarSearch( Problem problem, IHeuristicFunction heuristic )
{
this.m_problem = problem;
this.m_heuristic = heuristic;
}
public static int Main( string[] args )
{
if( args.Length != 1 ) {
ShowParams();
Console.WriteLine( "Press any key to exit." );
System.Console.ReadKey();
return -1;
}
var path = args[0];
if( !File.Exists( path ) ) {
ShowParams();
ShowFileSetup();
Console.WriteLine( "Press any key to exit." );
System.Console.ReadKey();
return -2;
}
execWatch = Stopwatch.StartNew();
Console.WriteLine( "building map..." );
string[] lines = File.ReadAllLines( path );
if( lines.Length < 6 ) {
ShowFileSetup();
return -3;
}
// first line has the x and then y size
SetRoomDimensions( lines[0] );
SetRobots( lines );
// 2 + Robots.Count lines appear before the rendevous point
SetRendevousPoint( lines[2 + Robots.Count] );
// where the floor plan starts in the text file
int depth = 3 + Robots.Count();
for( int j = DimY - 1; j >= 0; j-- ) {
char[] line = lines[depth].ToCharArray();
for( int i = 0; i < DimX; i++ ) {
int pointValue = ( int )Char.GetNumericValue( line[i] );
if( pointValue == 1 ) {
continue;
}
var label = i + "," + j;
ProblemMap.AddVertex( label, i, j );
AddLinks( i, j, label );
}
depth++;
}
Console.WriteLine( "Finished building." );
var fileNameStore = new ConcurrentBag<string>();
Parallel.ForEach( Robots, Robot => {
var start = string.Format(
"{0},{1}",
Robot.XCoord,
Robot.YCoord
);
Problem problem = new Problem(
start,
new ActionsFunction( ProblemMap ),
new ResultFunction(),
new GoalTest( Rendezvous ),
new SimpleStepCostFunction()
);
IHeuristicFunction hf =
new DirectPathHeuristicFunction( Rendezvous );
ISearch search = new AStarSearch( problem, hf );
stopWatch = Stopwatch.StartNew();
var fileName = string.Format(
"robot-Guid.-{0}.txt",
Guid.NewGuid()
);
fileNameStore.Add( fileName );
File.WriteAllText(
fileName,
string.Format(
"Robot starting at x: {0} and y: {1} ...\n",
Robot.XCoord,
Robot.YCoord
)
);
Console.WriteLine( string.Format(
"Robot starting at x: {0} and y: {1} ...\n",
Robot.XCoord,
Robot.YCoord
) );
File.AppendAllText(
fileName,
string.Format(
"Solution path for robot starting at ({0},{1}):\n",
Robot.XCoord,
Robot.YCoord
)
);
Console.WriteLine( string.Format(
"Solution path for robot starting at ({0},{1}):\n",
Robot.XCoord,
Robot.YCoord
) );
if( Robot.Equals( Rendezvous ) ) {
File.AppendAllText(
fileName,
"This robot is starting at the goal state."
);
} else {
var results = search.Search( problem );
if( results.Any() ) {
File.AppendAllText(
fileName,
string.Format(
"({0},{1}) -> ({2})\n",
Robot.XCoord,
Robot.YCoord,
string.Join(
") -> (",
results.Select(
m => m.TargetLocation
)
)
)
);
Console.WriteLine( "finished for robot starting at: " + Robot.XCoord + ", " + Robot.YCoord );
} else {
File.AppendAllText(
fileName,
"A path could not be found for this robot. " +
"It must be trapped!"
);
}
}
stopWatch.Stop();
// Get the elapsed time as a TimeSpan value.
TimeSpan ts = stopWatch.Elapsed;
// Format and display the TimeSpan value.
string elapsedTime = string.Format(
"{0:00}:{1:00}:{2:00}.{3:00}",
ts.Hours,
ts.Minutes,
ts.Seconds,
ts.Milliseconds
);
File.AppendAllText(
fileName,
string.Format( "RunTime: {0}\n", elapsedTime )
);
} );
MergeAndDeleteFiles( fileNameStore );
execWatch.Stop();
// Get the elapsed time as a TimeSpan value.
TimeSpan t = execWatch.Elapsed;
// Format and display the TimeSpan value.
string elapsed = string.Format(
"{0:00}:{1:00}:{2:00}.{3:00}",
t.Hours,
t.Minutes,
t.Seconds,
t.Milliseconds
);
Console.WriteLine(
string.Format( "Total runtime: {0}", elapsed )
);
#if DEBUG
Console.WriteLine( "Press any key to exit." );
System.Console.ReadKey();
#endif
return 0;
}