public bool FindPath(out List <Vector> movePositions, out List <ICommand> commands, out int iterationsCount)
{
var priorityQueue = new PriorityQueue <AStarState>();
var startState = new AStarState
{
DestroyedCell = Model.IsFull(BotPosition) ? BotPosition : null,
EndPosition = BotPosition,
Straight = true
};
var endState = new AStarState
{
StartPosition = End,
EndPosition = End,
Straight = false,
DestroyedCell = Matrix.IsFull(End) ? End : null
};
priorityQueue.Enqueue(0, startState);
priorityQueue.Enqueue(0, endState);
MinStates[Tuple.Create(startState.EndPosition, false)] = startState;
MinStates[Tuple.Create(endState.EndPosition, false)] = endState;
bool isEnd = false;
AStarState result = null;
AStarState endResult = null;
iterationsCount = 0;
var bestWeight = long.MaxValue;
while (!priorityQueue.Empty)
{
var state = priorityQueue.Dequeue();
if (MinStates.TryGetValue(Tuple.Create(state.EndPosition, state.DestroyedCell != null), out var minState) && minState.Weight < state.Weight)
{
continue;
}
foreach (var newState in GetSJumps(state).Concat(GetLJumps(state)).Concat(GetJumpsIntoFills(state)))
{
if (MinStates.TryGetValue(Tuple.Create(newState.EndPosition, newState.DestroyedCell != null), out var oldState) && oldState.Weight <= newState.Weight && newState.Straight == oldState.Straight)
{
continue;
}
if (oldState != null && newState.Straight != oldState.Straight)
{
if (newState.Straight)
{
result = newState;
endResult = oldState;
}
else
{
result = oldState;
endResult = newState;
}
if (newState.DestroyedCell == null)
{
isEnd = true;
break;
}
}
MinStates[Tuple.Create(newState.EndPosition, newState.DestroyedCell != null)] = newState;
priorityQueue.Enqueue(-newState.MaxPotentialWeight + (newState.Straight
? (End - newState.EndPosition).Euclidlen
: (BotPosition - newState.EndPosition).Euclidlen) / Matrix.R / 3, newState);
iterationsCount++;
}
if (isEnd)
{
break;
}
}
if (!isEnd)
{
movePositions = null;
commands = null;
return(false);
}
var states = result.GetStates().Reverse().Concat(endResult.GetStates()).ToList();
commands = states.SelectMany(state => state.GetCommands()).ToList();
movePositions = states.SelectMany(state => state.Straight ? state.GetUsedVectors() : state.GetUsedVectors().Reverse()).ToList();
return(true);
}
