public IEnumerable <AStarState> GetJumpsIntoFills(AStarState botState)
{
var currentWeight = botState.Weight;
foreach (var adjacent in botState.EndPosition.GetAdjacents())
{
if (!Matrix.Contains(adjacent) || Matrix.IsVoid(adjacent))
{
continue;
}
var weight = currentWeight + GetInFillWeight();
if (!GroundedChecker.CanRemove(new[] { adjacent, botState.DestroyedCell }.Where(c => c != null).ToHashSet()))
{
continue;
}
yield return(new AStarState
{
Move1 = adjacent - botState.EndPosition,
StartPosition = botState.EndPosition,
EndPosition = adjacent,
DestroyedCell = adjacent,
LastDestroyedCell = botState.DestroyedCell,
Weight = weight,
Dad = botState,
MaxPotentialWeight = weight + GetPotentialOptimalWeight(adjacent, botState.Straight),
Straight = botState.Straight
});
}
}
public IEnumerable <AStarState> GetSJumps(AStarState botState)
{
var botPosition = botState.EndPosition;
var currentWeight = botState.Weight;
foreach (var vector in new Vector(0, 0, 0).GetAdjacents())
{
for (int i = 1; i <= 15; i++)
{
if (botState.DestroyedCell != null && i > 1)
{
break;
}
var move = vector * i;
var position = botPosition + move;
if (!Matrix.Contains(position) || Matrix.IsFull(position) || IsForbiddenArea(position) || position == BotPosition)
{
break;
}
var weight = currentWeight + GetSMoveWeight(i);
yield return(new AStarState
{
LastDestroyedCell = botState.DestroyedCell,
Move1 = move,
Dad = botState,
StartPosition = botState.EndPosition,
EndPosition = position,
Weight = weight,
MaxPotentialWeight = weight + GetPotentialOptimalWeight(position, botState.Straight),
Straight = botState.Straight
});
}
}
}
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);
}
public IEnumerable <AStarState> GetLJumps(AStarState botState)
{
var ss = new[] { -1, 1, -2, 2, -3, 3, -4, 4, -5, 5 };
if (botState.DestroyedCell != null)
{
yield break;
}
var botPosition = botState.EndPosition;
var currentWeight = botState.Weight;
var moveVectors = new[]
{
new Vector(1, 0, 0),
new Vector(0, 1, 0),
new Vector(0, 0, 1)
};
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
if (i == j)
{
continue;
}
bool bannedNX = false;
bool bannedPX = false;
foreach (var x in ss)
{
var move1 = moveVectors[i] * x;
if (x < 0 && bannedNX || x > 0 && bannedPX)
{
continue;
}
if (!Matrix.Contains(botPosition + move1) || Matrix.IsFull(botPosition + move1) ||
IsForbiddenArea(botPosition + move1) || botPosition + move1 == BotPosition)
{
if (x < 0)
{
bannedNX = true;
}
else
{
bannedPX = true;
}
continue;
}
bool bannedNY = false;
bool bannedPY = false;
foreach (var y in ss)
{
var move2 = moveVectors[j] * y;
if (y < 0 && bannedNY || y > 0 && bannedPY)
{
continue;
}
if (!Matrix.Contains(botPosition + move1 + move2) || Matrix.IsFull(botPosition + move1 + move2) ||
IsForbiddenArea(botPosition + move1 + move2) || botPosition + move1 + move2 == BotPosition)
{
if (y < 0)
{
bannedNY = true;
}
else
{
bannedPY = true;
}
continue;
}
var position = botPosition + move1 + move2;
var weight = currentWeight + GetLMoveWeight(x, y);
var state = new AStarState
{
Move1 = move1,
Move2 = move2,
Dad = botState,
StartPosition = botState.EndPosition,
EndPosition = position,
Weight = weight,
MaxPotentialWeight = weight + GetPotentialOptimalWeight(position, botState.Straight),
Straight = botState.Straight
};
yield return(state);
}
}
}
}
}