public RobotLocator(InternalMap map, RMWorld world)
{
this.world = world;
this.map = map;
currentDirection = map.RobotId == TwoPlayersId.Left ? Direction.Right : Direction.Left;
realRobotAngle = currentDirection.ToAngle();
expectedRobotAngle = realRobotAngle;
}
private static void TryAddPosition(InternalMap map, Direction direction, InternalPoint position, int xOffset, int yOffset)
{
var availableDirections = map.AvailableDirectionsByCoordinates[position.X, position.Y];
var point = new InternalPoint(position.X + xOffset, position.Y + yOffset, direction);
if ( (availableDirections & direction)!= 0 && !handled.Contains(point))
{
AddPoint(point);
parents.SafeAdd(point, position);
}
}
public static Direction[] FindPath(InternalMap map, Point from, Point to)
{
var lastDirection = Bfs(map, from, to);
var directions = new List<Direction>();
var currentPoint = new InternalPoint(to.X, to.Y, lastDirection);
while (parents.ContainsKey(currentPoint))
{
directions.Add(currentPoint.Direction);
currentPoint = parents[currentPoint];
}
directions.Reverse();
return directions.ToArray();
}
private static Direction Bfs(InternalMap map, Point from, Point to)
{
queue = new Queue<InternalPoint>();
parents = new Dictionary<InternalPoint, InternalPoint>();
handled = new HashSet<InternalPoint>();
AddPoint(new InternalPoint(from.X, from.Y, Direction.No));
while (queue.Count > 0)
{
var position = queue.Dequeue();
if (position.X == to.X && position.Y == to.Y)
return position.Direction;
TryAddPosition(map, Direction.Down, position, 0, 1);
TryAddPosition(map, Direction.Up, position, 0, -1);
TryAddPosition(map, Direction.Left, position, -1, 0);
TryAddPosition(map, Direction.Right, position, 1, 0);
}
throw new Exception("Path not found.");
}
public static InternalMap BuildMap(this BotsSensorsData positionSensorsData)
{
var map = new InternalMap(positionSensorsData);
map.AvailableDirectionsByCoordinates = GetAvailableDirections(map.Walls);
return map;
}