public IEnumerable <Cuboid> TraverseFullCuboids()
{
if (_innerNode)
{
if (Count() == _bounds.Size)
{
yield return(_bounds);
}
else
{
foreach (var child in _children)
{
foreach (var cuboid in child.TraverseFullCuboids())
{
yield return(cuboid);
}
}
}
}
else
{
if (_leafs.Count > 0)
{
foreach (var leaf in _leafs)
{
yield return(Cuboid.FromPoints(
leaf.Point,
leaf.Point
));
}
}
}
}
public void FindsThreeStraightLinesIfThereIsObstacleInFrontOfMove()
{
var boundingBox = Cuboid.FromPoints(
Point3D.Origin(),
new Point3D(10, 10, 10));
var source = new Point3D(5, 5, 5);
var dest = new Point3D(5, 5, 7);
var obstacle = Cuboid.FromPoints(
new Point3D(5, 5, 6),
new Point3D(5, 5, 6)
);
var path = PathFinder.FindPathBetween(
source,
dest,
boundingBox,
new[] { obstacle });
var pathCommand1 = path[0] as StraightMoveCommand;
var pathCommand2 = path[1] as StraightMoveCommand;
var pathCommand3 = path[2] as StraightMoveCommand;
Assert.Equal(3, path.Count);
Assert.IsType <StraightMoveCommand>(path[0]);
Assert.IsType <StraightMoveCommand>(path[1]);
Assert.IsType <StraightMoveCommand>(path[2]);
Assert.Equal <Vector3D>(new Vector3D(1, 0, 0), pathCommand1.LongDiff);
Assert.Equal <Vector3D>(new Vector3D(0, 0, 2), pathCommand2.LongDiff);
Assert.Equal <Vector3D>(new Vector3D(-1, 0, 0), pathCommand3.LongDiff);
}
public static OctoTree <T> Create(int resolution)
{
return(Create(Cuboid.FromPoints(
Point3D.Origin(),
new Point3D(resolution - 1, resolution - 1, resolution - 1)
)));
}
public OctoTree <VoxelStatus> ToOctoTree()
{
var tree = OctoTree.OctoTree <VoxelStatus>
.Create(Cuboid.FromPoints(
Point3D.Origin(),
new Point3D(Resolution - 1, Resolution - 1, Resolution - 1)));
// Console.WriteLine($"Resolution: {Resolution}");
foreach (var fullVoxelPoint in Matrix.FullVoxels())
{
// Console.WriteLine($"Trying to add {fullVoxelPoint}");
tree.AddNode(fullVoxelPoint, VoxelStatus.Empty);
}
return(tree);
}
public void FindsStraightLineIfItExists()
{
var boundingBox = Cuboid.FromPoints(
Point3D.Origin(),
new Point3D(10, 10, 10));
var source = new Point3D(5, 5, 5);
var dest = new Point3D(5, 8, 5);
var path = PathFinder.FindPathBetween(
source,
dest,
boundingBox);
var pathCommand = path[0] as StraightMoveCommand;
Assert.Equal(1, path.Count);
Assert.IsType <StraightMoveCommand>(path[0]);
Assert.Equal <Vector3D>(new Vector3D(0, 3, 0), pathCommand.LongDiff);
}
public void FindsTwoStraightLinesIfTheyExists()
{
var boundingBox = Cuboid.FromPoints(
Point3D.Origin(),
new Point3D(10, 10, 10));
var source = new Point3D(5, 5, 5);
var dest = new Point3D(5, 1, 6);
var path = PathFinder.FindPathBetween(
source,
dest,
boundingBox);
var pathCommand1 = path[0] as StraightMoveCommand;
var pathCommand2 = path[1] as StraightMoveCommand;
Assert.Equal(2, path.Count);
Assert.IsType <StraightMoveCommand>(path[0]);
Assert.IsType <StraightMoveCommand>(path[1]);
Assert.Equal <Vector3D>(new Vector3D(0, -4, 0), pathCommand1.LongDiff);
Assert.Equal <Vector3D>(new Vector3D(0, 0, 1), pathCommand2.LongDiff);
}
public IEnumerable <ICommand> Solve()
{
buildingMatrix = new CorrectComponentTrackingMatrix(new bool[N, N, N]);
var(transformedTargetMatrix, stickPositions) = TransformMatrix(targetMatrix);
var(cloneCommands, initialBots) =
fast
? Clone2(new Grid(grid.CountX, 2, minX, minZ, maxX, maxZ))
: Clone(2 * grid.CountX, new Grid(grid.CountX, 2, minX, minZ, maxX, maxZ));
foreach (var command in cloneCommands)
{
yield return(command);
}
var botQueues = new List <Queue <ICommand> >();
for (var i = 0; i < initialBots.Count; i++)
{
botQueues.Add(new Queue <ICommand>());
}
var botsToGenerateCommands = initialBots.ToList();
var botCount = botsToGenerateCommands.Count;
for (var y = 0; y <= maxY; y++)
{
for (var botId = 0; botId < botCount / 2; botId++)
{
var nearId = botId;
var farId = botId + botCount / 2;
foreach (var(nearCommand, farCommand) in FillLayer(transformedTargetMatrix, botsToGenerateCommands[nearId], botsToGenerateCommands[farId]))
{
var nearBeforeTransform = grid.GetCellId(botsToGenerateCommands[nearId]);
var farBeforeTransform = grid.GetCellId(botsToGenerateCommands[farId]);
botQueues[nearId].Enqueue(nearCommand);
botQueues[farId].Enqueue(farCommand);
if (nearCommand is SMove sMove)
{
botsToGenerateCommands[nearId] += sMove.Shift;
}
if (nearCommand is LMove lMove)
{
botsToGenerateCommands[nearId] += lMove.firstShift.Shift + lMove.secondShift.Shift;
}
if (farCommand is SMove sMove1)
{
botsToGenerateCommands[farId] += sMove1.Shift;
}
if (farCommand is LMove lMove1)
{
botsToGenerateCommands[farId] += lMove1.firstShift.Shift + lMove1.secondShift.Shift;
}
if (!nearBeforeTransform.Equals(grid.GetCellId(botsToGenerateCommands[nearId])))
{
throw new Exception("Wrong zone");
}
if (!farBeforeTransform.Equals(grid.GetCellId(botsToGenerateCommands[farId])))
{
throw new Exception("Wrong zone");
}
}
}
}
for (var botId = 0; botId < botCount / 2; botId++)
{
foreach (var command in RemoveSticks(stickPositions, botsToGenerateCommands[botId]))
{
botQueues[botId].Enqueue(command);
if (command is SMove sMove)
{
botsToGenerateCommands[botId] += sMove.Shift;
}
if (command is LMove lMove)
{
botsToGenerateCommands[botId] += lMove.firstShift.Shift + lMove.secondShift.Shift;
}
}
}
var botsToEvaluate = initialBots.ToList();
bool isHighEnergy = false;
bool firstHigh = false;
while (botQueues.Any(x => x.Count > 0))
{
var commands = new List <ICommand>();
if (isHighEnergy && !firstHigh && !buildingMatrix.HasNonGroundedVoxels)
{
commands = new ICommand[] { new Flip() }.Concat(Enumerable.Repeat <ICommand>(new Wait(), botsToEvaluate.Count - 1)).ToList();
isHighEnergy = false;
foreach (var command in commands)
{
yield return(command);
}
continue;
}
bool[] shouldWait = new bool[botCount];
firstHigh = false;
for (var i = 0; i < botQueues.Count; i++)
{
if (botQueues[i].Count == 0)
{
continue;
}
if (botQueues[i].Peek() is Fill fillCommand)
{
var fillPosition = botsToEvaluate[i] + fillCommand.Shift;
if (!CanFill(buildingMatrix.Voxels, fillPosition) && !isHighEnergy)
{
shouldWait[i] = true;
shouldWait[GetPartnerId(i)] = true;
}
}
else if (botQueues[i].Peek() is GFill gFillCommand)
{
var filledCells = new List <Vec>();
foreach (var cell in Cuboid.FromPoints(botsToEvaluate[i] + gFillCommand.NearShift, botsToEvaluate[i] + gFillCommand.NearShift + gFillCommand.FarShift).AllPoints())
{
if (!buildingMatrix[cell])
{
filledCells.Add(cell);
}
buildingMatrix[cell] = true;
}
if (buildingMatrix.HasNonGroundedVoxels && !isHighEnergy)
{
foreach (var filledCell in filledCells)
{
buildingMatrix[filledCell] = false;
}
shouldWait[i] = true;
shouldWait[GetPartnerId(i)] = true;
}
}
else if (botQueues[i].Peek() is Voidd voidCommand)
{
var voidPosition = botsToEvaluate[i] + voidCommand.Shift;
if (!buildingMatrix.CanVoidCell(voidPosition) && !isHighEnergy)
{
shouldWait[i] = true;
shouldWait[GetPartnerId(i)] = true;
}
}
}
for (var i = 0; i < botQueues.Count; i++)
{
if (botQueues[i].Count == 0 || shouldWait[i])
{
commands.Add(new Wait());
continue;
}
if (botQueues[i].Peek() is Fill fillCommand)
{
var fillPosition = botsToEvaluate[i] + fillCommand.Shift;
buildingMatrix[fillPosition] = true;
commands.Add(botQueues[i].Dequeue());
}
else if (botQueues[i].Peek() is GFill gFillCommand)
{
foreach (var cell in Cuboid.FromPoints(botsToEvaluate[i] + gFillCommand.NearShift, botsToEvaluate[i] + gFillCommand.NearShift + gFillCommand.FarShift).AllPoints())
{
buildingMatrix[cell] = true;
}
commands.Add(botQueues[i].Dequeue());
}
else if (botQueues[i].Peek() is Voidd voidCommand)
{
var voidPosition = botsToEvaluate[i] + voidCommand.Shift;
buildingMatrix[voidPosition] = false;
commands.Add(botQueues[i].Dequeue());
}
else
{
commands.Add(botQueues[i].Dequeue());
}
}
if (commands.All(x => x is Wait))
{
firstHigh = isHighEnergy = true;
commands[0] = new Flip();
}
for (var i = 0; i < commands.Count; i++)
{
if (commands[i] is SMove sMove)
{
botsToEvaluate[i] += sMove.Shift;
}
if (commands[i] is LMove lMove)
{
botsToEvaluate[i] += lMove.firstShift.Shift + lMove.secondShift.Shift;
}
yield return(commands[i]);
}
}
foreach (var command in fast ? GoHome2(botsToEvaluate) : GoHome(botsToEvaluate))
{
yield return(command);
}
}
private bool DivideSelf()
{
var minPoint = _bounds.MinPoint;
var maxPoint = _bounds.MaxPoint;
var _size_x = (maxPoint.X - minPoint.X + 1) / 2;
var _size_y = (maxPoint.Y - minPoint.Y + 1) / 2;
var _size_z = (maxPoint.Z - minPoint.Z + 1) / 2;
_children = new OctoTreeNode <T> [8];
_children[0] = new OctoTreeNode <T>(
Cuboid.FromPoints(
new Point3D(minPoint.X, minPoint.Y, minPoint.Z),
new Point3D(maxPoint.X - _size_x, maxPoint.Y - _size_y, maxPoint.Z - _size_z)
),
_capacity);
_children[1] = new OctoTreeNode <T>(
Cuboid.FromPoints(
new Point3D(minPoint.X, minPoint.Y, minPoint.Z + _size_z),
new Point3D(maxPoint.X - _size_x, maxPoint.Y - _size_y, maxPoint.Z)
),
_capacity);
_children[2] = new OctoTreeNode <T>(
Cuboid.FromPoints(
new Point3D(minPoint.X + _size_x, minPoint.Y, minPoint.Z + _size_z),
new Point3D(maxPoint.X, maxPoint.Y - _size_y, maxPoint.Z)
),
_capacity);
_children[3] = new OctoTreeNode <T>(
Cuboid.FromPoints(
new Point3D(minPoint.X + _size_x, minPoint.Y, minPoint.Z),
new Point3D(maxPoint.X, maxPoint.Y - _size_y, maxPoint.Z - _size_z)
),
_capacity);
_children[4] = new OctoTreeNode <T>(
Cuboid.FromPoints(
new Point3D(minPoint.X + _size_x, minPoint.Y + _size_y, minPoint.Z),
new Point3D(maxPoint.X, maxPoint.Y, maxPoint.Z - _size_z)
),
_capacity);
_children[5] = new OctoTreeNode <T>(
Cuboid.FromPoints(
new Point3D(minPoint.X + _size_x, minPoint.Y + _size_y, minPoint.Z + _size_z),
new Point3D(maxPoint.X, maxPoint.Y, maxPoint.Z)
),
_capacity);
_children[6] = new OctoTreeNode <T>(
Cuboid.FromPoints(
new Point3D(minPoint.X, minPoint.Y + _size_y, minPoint.Z + _size_z),
new Point3D(maxPoint.X - _size_x, maxPoint.Y, maxPoint.Z)
),
_capacity);
_children[7] = new OctoTreeNode <T>(
Cuboid.FromPoints(
new Point3D(minPoint.X, minPoint.Y + _size_y, minPoint.Z),
new Point3D(maxPoint.X - _size_x, maxPoint.Y, maxPoint.Z - _size_z)
),
_capacity);
foreach (var leaf in _leafs)
{
foreach (var child in _children)
{
if (child.AddNode(leaf.Point, leaf.Value))
{
break;
}
}
}
_innerNode = true;
_leafs = null;
return(true);
}
public Cuboid[] GetObstaclesAgainstStraightMove(Point3D source)
{
var vector = Vector3D.FromPoint(source);
var padBoundBox = PadBoundBox;
var obstacles = new List <Cuboid>();
foreach (var dx in Enumerable.Range(1, 15))
{
var newVector = vector + Vector3D.FromPoint(new Point3D(dx, 0, 0));
var newPoint = newVector.ToPoint();
if (padBoundBox.Contains(newPoint) &&
this.SceneState.Matrix[newPoint] == VoxelStatus.Full)
{
obstacles.Add(Cuboid.FromPoints(newPoint, newPoint));
break;
}
}
foreach (var dx in Enumerable.Range(1, 15))
{
var newVector = vector + Vector3D.FromPoint(new Point3D(-dx, 0, 0));
var newPoint = newVector.ToPoint();
if (padBoundBox.Contains(newPoint) &&
this.SceneState.Matrix[newPoint] == VoxelStatus.Full)
{
obstacles.Add(Cuboid.FromPoints(newPoint, newPoint));
break;
}
}
foreach (var dy in Enumerable.Range(1, 15))
{
var newVector = vector + Vector3D.FromPoint(new Point3D(0, dy, 0));
var newPoint = newVector.ToPoint();
if (padBoundBox.Contains(newPoint) &&
this.SceneState.Matrix[newPoint] == VoxelStatus.Full)
{
obstacles.Add(Cuboid.FromPoints(newPoint, newPoint));
break;
}
}
foreach (var dy in Enumerable.Range(1, 15))
{
var newVector = vector + Vector3D.FromPoint(new Point3D(0, -dy, 0));
var newPoint = newVector.ToPoint();
if (padBoundBox.Contains(newPoint) &&
this.SceneState.Matrix[newPoint] == VoxelStatus.Full)
{
obstacles.Add(Cuboid.FromPoints(newPoint, newPoint));
break;
}
}
foreach (var dz in Enumerable.Range(1, 15))
{
var newVector = vector + Vector3D.FromPoint(new Point3D(0, 0, dz));
var newPoint = newVector.ToPoint();
if (padBoundBox.Contains(newPoint) &&
this.SceneState.Matrix[newPoint] == VoxelStatus.Full)
{
obstacles.Add(Cuboid.FromPoints(newPoint, newPoint));
break;
}
}
foreach (var dz in Enumerable.Range(1, 15))
{
var newVector = vector + Vector3D.FromPoint(new Point3D(0, 0, -dz));
var newPoint = newVector.ToPoint();
if (padBoundBox.Contains(newPoint) &&
this.SceneState.Matrix[newPoint] == VoxelStatus.Full)
{
obstacles.Add(Cuboid.FromPoints(newPoint, newPoint));
break;
}
}
return(obstacles.ToArray());
}
public Cuboid[] GetObstacles()
{
return(SceneState.Matrix.FullVoxels()
.Select(p => Cuboid.FromPoints(p, p))
.ToArray());
}
public static List <BotCommand> FindPathBetweenWithOnlyStraightMoves(
Point3D source,
Point3D destination,
Scene scene)
{
var boundingBox = scene.PadBoundBox;
var destinationVector = Vector3D.FromPoint(destination);
var sourceVector = Vector3D.FromPoint(source);
var priorityQueue = new IntervalHeap <Vector3D>(
new PriorityQueueComparer(destinationVector));
var visited = new System.Collections.Generic.HashSet <Vector3D>();
var g_of_x = new Dictionary <Vector3D, int>();
var parent = new Dictionary <Vector3D, PathTempInfo>();
// var f_of_x = new Dictionary<Vector3D, int>();
priorityQueue.Add(sourceVector);
g_of_x[sourceVector] = 0;
parent[sourceVector] = new PathTempInfo();
// f_of_x[sourceVector] = DistanceCost(sourceVector, destinationVector);
while (priorityQueue.Count != 0)
{
var nextPoint = priorityQueue.DeleteMin();
if (nextPoint.ToPoint() == destination)
{
// We have found the path!
return(GeneratePathFrom(parent, sourceVector, destinationVector));
}
var obstacles = scene.GetObstaclesAgainstStraightMove(nextPoint.ToPoint());
visited.Add(nextPoint);
// Console.WriteLine($"Enumerating paths from {nextPoint}");
foreach (var reachableStruct in EnumerateMoves(nextPoint))
{
// Console.WriteLine($"Checking to {reachableStruct.First} with type {reachableStruct.MoveType}");
var reachable = reachableStruct.First + nextPoint;
var reachablePoint = reachable.ToPoint();
var pathObstacle = Cuboid.FromPoints(nextPoint.ToPoint(), reachablePoint);
if (!boundingBox.Contains(reachablePoint))
{
continue;
}
if (StraightPathBoundBoxIntersectsObstacles(pathObstacle, obstacles))
{
continue;
}
var tentativeScore = g_of_x[nextPoint] +
DistanceCost(nextPoint, reachable);
if (visited.Contains(reachable) &&
tentativeScore >= g_of_x.GetValueOrDefault(reachable, 0))
{
continue;
}
if (!visited.Contains(reachable) ||
tentativeScore < g_of_x.GetValueOrDefault(reachable, 0))
{
parent[reachable] = new PathTempInfo
{
Parent = nextPoint,
Move = reachableStruct
};
g_of_x[reachable] = tentativeScore;
//f_of_x[reachable] = tentativeScore + DistanceCost(reachable, destinationVector);
if (!priorityQueue.Contains(reachable))
{
priorityQueue.Add(reachable);
}
}
}
}
return(null);
}
