public List <AgentCommand> CreateOnlyFirstSolutionCommand(HighlevelMove move, State currentState, int agentIndex)
{
var box = move.MoveThis;
// Make all blockages into "fake" walls
foreach (Entity e in currentState.Entities)
{
Level.AddWall(e.Pos);
}
Level.RemoveWall(box.Pos);
Level.RemoveWall(move.ToHere);
List <AgentCommand> result = new List <AgentCommand>();
if (move.UsingThisAgent.HasValue)
{
int boxIndex = Array.IndexOf(Boxes, move.MoveThis);
Level.RemoveWall(move.UsingThisAgent.Value.Pos);
result = CreateSolutionCommands(move, move.ToHere, agentIndex, boxIndex);
}
else
{
var agentToPos = RunAStar(move.MoveThis.Pos, move.ToHere);
MoveToLocation(agentToPos, result, agentIndex);
}
Level.ResetWalls();
return(result);
}
private static void VerifyCommands(Level level, HighlevelMove move, List <AgentCommand> expectedCommands)
{
List <AgentCommands> agentCommands = new LessNaiveSolver(level, level, new List <HighlevelMove>()
{
move
}).Solve();
List <AgentCommand> actualCommands = agentCommands.First().Commands;
CollectionAssert.AreEqual(expectedCommands, actualCommands);
}
private static void VerifyMoveAgentToGoalCreator(string levelString, List <AgentCommand> expectedCommands)
{
Level level = TestTools.StringToLevel(levelString);
Level clearedLevel = TestTools.StringToLevel(levelString.Replace('G', ' '));
Entity agent = level.InitialState.Entities.Single(x => x.Type == '0');
Entity goal = level.InitialState.Entities.Single(x => x.Type == 'G');
HighlevelMove move = new HighlevelMove(clearedLevel.InitialState, agent, goal.Pos, null, null);
VerifyCommands(clearedLevel, move, expectedCommands);
}
private static void VerifyMoveBoxToGoalCreator(string levelString, List <AgentCommand> expectedCommands)
{
Level level = TestTools.StringToLevel(levelString);
Level clearedLevel = TestTools.StringToLevel(levelString.Replace('G', ' ').Replace('F', ' '));
Entity agent = level.InitialState.Entities.Single(x => x.Type == '0');
Entity box = level.InitialState.Entities.Single(x => x.Type == 'B');
Entity goal = level.InitialState.Entities.Single(x => x.Type == 'G');
Point agentFinalPos = level.InitialState.Entities.Single(x => x.Type == 'F').Pos;
HighlevelMove move = new HighlevelMove(clearedLevel.InitialState, box, goal.Pos, agent, agentFinalPos);
VerifyCommands(clearedLevel, move, expectedCommands);
}
private void FindDistantTurningPoint(
HighlevelMove plan,
ref Point agentNextToBox,
Point agentEndPos,
ref Point?turnPoint,
ref Point turnIntoPoint,
Point boxPos,
Point goalPos,
List <AgentCommand> commands,
int agentIndex,
int boxIndex,
ref List <Point> boxToAgentEnd,
ref bool startPull,
ref bool endPull,
ref int count,
ref int skipFirst
)
{
turnPoint = plan.UTurnPos.Value;
// Find spot beside turning point.
foreach (Point dirDelta in Direction.NONE.DirectionDeltas())
{
Point dirP = turnPoint.Value + dirDelta;
if (!Level.IsWall(dirP) && dirP != agentNextToBox)
{
turnIntoPoint = turnPoint.Value + dirDelta;
break;
}
}
// Reach turning point.
var pathToTurnIntoPoint = RunAStar(boxPos, turnIntoPoint);
// If A* doesn't go through selected turn(into) points
//if (pathToTurnIntoPoint[pathToTurnIntoPoint.Count - 2] != turnPoint)
//{
// List<Point> blocks = new List<Point>();
// while (pathToTurnIntoPoint[pathToTurnIntoPoint.Count - 2] != turnPoint)
// {
// Point p = pathToTurnIntoPoint[pathToTurnIntoPoint.Count - 2];
// Level.AddWall(p);
// blocks.Add(p);
// pathToTurnIntoPoint = RunAStar(boxPos, turnIntoPoint);
// }
// foreach (Point p in blocks)
// Level.RemoveWall(p);
//}
startPull = !pathToTurnIntoPoint.Contains(agentNextToBox);
if (startPull)
{
//if (!pathToTurnIntoPoint.Contains(agentNextToBox))
PushOnPath(pathToTurnIntoPoint, agentNextToBox, commands, agentIndex, boxIndex);
Agents[agentIndex] = Agents[agentIndex].Move(turnPoint.Value);
boxToAgentEnd = RunAStar(turnIntoPoint, agentEndPos);
}
else
{
// SARegExAZ is an excellent example... of a mess.
PullOnPath(pathToTurnIntoPoint, commands, agentIndex, boxIndex);
Agents[agentIndex] = Agents[agentIndex].Move(turnIntoPoint);
boxToAgentEnd = RunAStar(turnPoint.Value, agentEndPos);
}
agentNextToBox = Agents[agentIndex].Pos; // Agent is always next to box.
endPull = boxToAgentEnd.Contains(goalPos);
startPull = !endPull;
count = 0;
skipFirst = startPull ? 1 : 0;
foreach (var pos in boxToAgentEnd.Skip(skipFirst))
{
count++;
if (!IsCorridor(pos))
{
turnPoint = pos;
break;
}
}
count += skipFirst;
}
public List <AgentCommand> CreateSolutionCommands(HighlevelMove plan, Point goalPos, int agentIndex, int boxIndex)
{
List <AgentCommand> commands = new List <AgentCommand>();
var agent = plan.UsingThisAgent.Value;
var agentEndPos = plan.AgentFinalPos.Value;
var box = plan.MoveThis;
List <Point> agentToBox = null;
// Avoids stupid square problems.
if (Point.ManhattenDistance(box.Pos, goalPos) == 1)
{
if (IsStraightCorridor(goalPos) || IsStraightCorridor(box.Pos))
{
// Do nothing
}
else if (box.Pos == agentEndPos)
{
// Force agent's pathfinding to box to avoid the goal.
Level.AddWall(goalPos);
}
else if (Point.ManhattenDistance(agentEndPos, goalPos) == 1)
{ // Force agent's position next to box onto the goal.
Level.AddWall(box.Pos);
agentToBox = RunAStar(agent.Pos, goalPos);
agentToBox.Add(box.Pos);
}
}
if (agentToBox == null)
{
agentToBox = RunAStar(agent.Pos, box.Pos);
}
Level.RemoveWall(goalPos);
Level.RemoveWall(box.Pos);
//Remove box location from the path as the path should end next to the box
agentToBox.RemoveAt(agentToBox.Count - 1);
MoveToLocation(agentToBox, commands, agentIndex);
Point agentNextToBox = agentToBox[agentToBox.Count - 1];
var boxToAgentEnd = RunAStar(box.Pos, agentEndPos);
if (boxToAgentEnd.Count == 3 && agentNextToBox == goalPos && boxToAgentEnd[1] != goalPos)
{
boxToAgentEnd[1] = goalPos; // If pathfinding dun goofed, then force agent to pull through goal position when it OBVIOUSLY should.
}
bool startPull = boxToAgentEnd.Contains(agentNextToBox);
bool endPull = boxToAgentEnd.Contains(goalPos);
if (agentNextToBox == goalPos) // If agent blocks goal position
{
startPull = true; // Forces agent to locate distant U-turn location, pull at U-turn, and push to goal.
}
List <Point> firstPart = null;
List <Point> secondPart = null;
if (startPull != endPull)
{
Point?turnPoint = null;
Point turnIntoPoint = new Point();
//Find somewhere to turn around
int count = 0;
int skipFirst = startPull ? 1 : 0;
foreach (var pos in boxToAgentEnd.Skip(skipFirst))
{
count++;
if (!IsCorridor(pos))
{
turnPoint = pos;
break;
}
}
count += skipFirst;
// If we failed to find a turn point on the route
if (!turnPoint.HasValue)
{
FindDistantTurningPoint(plan, ref agentNextToBox, agentEndPos, ref turnPoint, ref turnIntoPoint, box.Pos, goalPos, commands, agentIndex, boxIndex, ref boxToAgentEnd, ref startPull, ref endPull, ref count, ref skipFirst);
}
turnIntoPoint = FindSpaceToTurn(boxToAgentEnd, turnPoint.Value, goalPos, agentNextToBox);
firstPart = new List <Point>();
firstPart.AddRange(boxToAgentEnd.Take(count));
firstPart.Add(turnIntoPoint);
secondPart = new List <Point>();
secondPart.Add(turnIntoPoint);
secondPart.AddRange(boxToAgentEnd.Skip(count - 1));
if (!endPull)
{
secondPart.Add(goalPos);
}
if (startPull && !endPull)
{
secondPart.RemoveAt(0);
}
}
else
{
firstPart = new List <Point>();
firstPart.AddRange(boxToAgentEnd);
if (!startPull)
{
firstPart.Add(goalPos);
}
}
Point?firstPartAgentEndPos = null;
if (startPull)
{
PullOnPath(firstPart, commands, agentIndex, boxIndex);
if (secondPart != null)
{
firstPartAgentEndPos = firstPart.Last();
}
}
else
{
PushOnPath(firstPart, agentNextToBox, commands, agentIndex, boxIndex);
if (secondPart != null)
{
firstPartAgentEndPos = firstPart[firstPart.Count - 2];
}
}
if (secondPart != null)
{
if (endPull)
{
PullOnPath(secondPart, commands, agentIndex, boxIndex);
}
else
{
PushOnPath(secondPart, firstPartAgentEndPos.Value, commands, agentIndex, boxIndex);
}
}
return(commands);
}
