/**
* Copyright (c) 2011-2014, SpaceToad and the BuildCraft Team
* http://www.mod-buildcraft.com
*
* BuildCraft is distributed under the terms of the Minecraft Mod Public
* License 1.0, or MMPL. Please check the contents of the license located in
* http://www.mod-buildcraft.com/MMPL-1.0.txt
*/
package buildcraft.core.utils;
import java.util.ArrayList;
import java.util.Collection;
import java.util.HashMap;
import java.util.LinkedList;
import net.minecraft.world.World;
import buildcraft.api.core.BlockIndex;
import buildcraft.api.core.BuildCraftAPI;
import buildcraft.api.core.IZone;
import buildcraft.core.robots.IBlockFilter;
/**
* This class implements a 3D path finding based on the A* algorithm, following
* guidelines documented on http://www.policyalmanac.org/games/aStarTutorial.htm
* .
*/
public class PathFinding {
public static int PATH_ITERATIONS = 1000;
private World world;
private BlockIndex start;
private BlockIndex end;
private BlockIndex boxEnd;
private IBlockFilter pathFound;
private float maxDistance = -1;
private float sqrMaxDistance = -1;
private IZone zone;
private double maxDistanceToEnd = 0;
private HashMap<BlockIndex, Node> openList = new HashMap<BlockIndex, PathFinding.Node>();
private HashMap<BlockIndex, Node> closedList = new HashMap<BlockIndex, PathFinding.Node>();
private Node nextIteration;
private LinkedList<BlockIndex> result;
private boolean endReached = false;
public PathFinding(World iWorld, BlockIndex iStart, BlockIndex iEnd) {
world = iWorld;
start = iStart;
end = iEnd;
Node startNode = new Node();
startNode.parent = null;
startNode.movementCost = 0;
startNode.destinationCost = distance(start, end);
startNode.totalWeight = startNode.movementCost + startNode.destinationCost;
startNode.index = iStart;
openList.put(start, startNode);
nextIteration = startNode;
}
public PathFinding(World iWorld, BlockIndex iStart, BlockIndex iEnd, double iMaxDistanceToEnd) {
this(iWorld, iStart, iEnd);
maxDistanceToEnd = iMaxDistanceToEnd;
}
// TODO: It's probably more efficient to start a search first, and then to
// compute the path, instead of computing all possible path from the get
// go.
public PathFinding(World iWorld, BlockIndex iStart, IBlockFilter iPathFound, float iMaxDistance, IZone iZone) {
world = iWorld;
start = iStart;
pathFound = iPathFound;
Node startNode = new Node();
startNode.parent = null;
startNode.movementCost = 0;
startNode.destinationCost = 0;
startNode.totalWeight = startNode.movementCost + startNode.destinationCost;
startNode.index = iStart;
openList.put(start, startNode);
nextIteration = startNode;
maxDistance = iMaxDistance;
sqrMaxDistance = maxDistance * maxDistance;
zone = iZone;
}
public void iterate() {
iterate(PATH_ITERATIONS);
}
public void iterate(int itNumber) {
for (int i = 0; i < itNumber; ++i) {
if (nextIteration == null) {
return;
}
if (endReached) {
result = new LinkedList<BlockIndex>();
while (nextIteration != null) {
result.addFirst(nextIteration.index);
nextIteration = nextIteration.parent;
}
return;
} else {
nextIteration = iterate(nextIteration);
}
}
}
public boolean isDone() {
return nextIteration == null;
}
public LinkedList<BlockIndex> getResult() {
if (result != null) {
return result;
} else {
return new LinkedList<BlockIndex>();
}
}
private Node iterate(Node from) {
openList.remove(from.index);
closedList.put(from.index, from);
ArrayList<Node> nodes = new ArrayList<Node>();
byte[][][] resultMoves = movements(from);
for (int dx = -1; dx <= +1; ++dx) {
for (int dy = -1; dy <= +1; ++dy) {
for (int dz = -1; dz <= +1; ++dz) {
if (resultMoves[dx + 1][dy + 1][dz + 1] == 0) {
continue;
}
int x = from.index.x + dx;
int y = from.index.y + dy;
int z = from.index.z + dz;
Node nextNode = new Node();
nextNode.parent = from;
nextNode.index = new BlockIndex(x, y, z);
if (resultMoves[dx + 1][dy + 1][dz + 1] == 2) {
endReached = true;
return nextNode;
}
nextNode.movementCost = from.movementCost + distance(nextNode.index, from.index);
if (end != null) {
nextNode.destinationCost = distance(nextNode.index, end);
} else if (zone != null) {
if (zone.contains(x, y, z)) {
nextNode.destinationCost = 0;
} else {
nextNode.destinationCost = zone.distanceTo(nextNode.index);
}
} else {
nextNode.destinationCost = 0;
}
nextNode.totalWeight = nextNode.movementCost + nextNode.destinationCost;
if (closedList.containsKey(nextNode.index)) {
continue;
} else if (openList.containsKey(nextNode.index)) {
Node tentative = openList.get(nextNode.index);
if (tentative.movementCost < nextNode.movementCost) {
nextNode = tentative;
} else {
openList.put(nextNode.index, nextNode);
}
} else {
openList.put(nextNode.index, nextNode);
}
nodes.add(nextNode);
}
}
}
nodes.addAll(openList.values());
return findSmallerWeight(nodes);
}
private Node findSmallerWeight(Collection<Node> collection) {
Node found = null;
for (Node n : collection) {
if (found == null) {
found = n;
} else if (n.totalWeight < found.totalWeight) {
found = n;
}
}
return found;
}
private static class Node {
public Node parent;
public double movementCost;
public double destinationCost;
public double totalWeight;
public BlockIndex index;
}
private static double distance(BlockIndex i1, BlockIndex i2) {
double dx = (double) i1.x - (double) i2.x;
double dy = (double) i1.y - (double) i2.y;
double dz = (double) i1.z - (double) i2.z;
return Math.sqrt(dx * dx + dy * dy + dz * dz);
}
private boolean endReached(int x, int y, int z) {
if (zone != null && !zone.contains(x, y, z)) {
return false;
} else if (pathFound != null) {
return pathFound.matches(world, x, y, z);
} else {
if (maxDistanceToEnd == 0) {
return end.x == x && end.y == y && end.z == z;
} else {
return BuildCraftAPI.isSoftBlock(world, x, y, z)
&& distance(new BlockIndex(x, y, z), end) <= maxDistanceToEnd;
}
}
}
private byte[][][] movements(Node from) {
byte[][][] resultMoves = new byte[3][3][3];
for (int dx = -1; dx <= +1; ++dx) {
for (int dy = -1; dy <= +1; ++dy) {
for (int dz = -1; dz <= +1; ++dz) {
int x = from.index.x + dx;
int y = from.index.y + dy;
int z = from.index.z + dz;
if (endReached(x, y, z)) {
resultMoves[dx + 1][dy + 1][dz + 1] = 2;
} else if (!BuildCraftAPI.isSoftBlock(world, x, y, z)) {
resultMoves[dx + 1][dy + 1][dz + 1] = 0;
} else {
resultMoves[dx + 1][dy + 1][dz + 1] = 1;
}
}
}
}
resultMoves[1][1][1] = 0;
if (resultMoves[0][1][1] == 0) {
for (int i = 0; i <= 2; ++i) {
for (int j = 0; j <= 2; ++j) {
resultMoves[0][i][j] = 0;
}
}
}
if (resultMoves[2][1][1] == 0) {
for (int i = 0; i <= 2; ++i) {
for (int j = 0; j <= 2; ++j) {
resultMoves[2][i][j] = 0;
}
}
}
if (resultMoves[1][0][1] == 0) {
for (int i = 0; i <= 2; ++i) {
for (int j = 0; j <= 2; ++j) {
resultMoves[i][0][j] = 0;
}
}
}
if (resultMoves[1][2][1] == 0) {
for (int i = 0; i <= 2; ++i) {
for (int j = 0; j <= 2; ++j) {
resultMoves[i][2][j] = 0;
}
}
}
if (resultMoves[1][1][0] == 0) {
for (int i = 0; i <= 2; ++i) {
for (int j = 0; j <= 2; ++j) {
resultMoves[i][j][0] = 0;
}
}
}
if (resultMoves[1][1][2] == 0) {
for (int i = 0; i <= 2; ++i) {
for (int j = 0; j <= 2; ++j) {
resultMoves[i][j][2] = 0;
}
}
}
if (resultMoves[0][0][1] == 0) {
resultMoves[0][0][0] = 0;
resultMoves[0][0][2] = 0;
}
if (resultMoves[0][2][1] == 0) {
resultMoves[0][2][0] = 0;
resultMoves[0][2][2] = 0;
}
if (resultMoves[2][0][1] == 0) {
resultMoves[2][0][0] = 0;
resultMoves[2][0][2] = 0;
}
if (resultMoves[2][2][1] == 0) {
resultMoves[2][2][0] = 0;
resultMoves[2][2][2] = 0;
}
if (resultMoves[0][1][0] == 0) {
resultMoves[0][0][0] = 0;
resultMoves[0][2][0] = 0;
}
if (resultMoves[0][1][2] == 0) {
resultMoves[0][0][2] = 0;
resultMoves[0][2][2] = 0;
}
if (resultMoves[2][1][0] == 0) {
resultMoves[2][0][0] = 0;
resultMoves[2][2][0] = 0;
}
if (resultMoves[2][1][2] == 0) {
resultMoves[2][0][2] = 0;
resultMoves[2][2][2] = 0;
}
if (resultMoves[1][0][0] == 0) {
resultMoves[0][0][0] = 0;
resultMoves[2][0][0] = 0;
}
if (resultMoves[1][0][2] == 0) {
resultMoves[0][0][2] = 0;
resultMoves[2][0][2] = 0;
}
if (resultMoves[1][2][0] == 0) {
resultMoves[0][2][0] = 0;
resultMoves[2][2][0] = 0;
}
if (resultMoves[1][2][2] == 0) {
resultMoves[0][2][2] = 0;
resultMoves[2][2][2] = 0;
}
if (maxDistance != -1) {
for (int dx = -1; dx <= +1; ++dx) {
for (int dy = -1; dy <= +1; ++dy) {
for (int dz = -1; dz <= +1; ++dz) {
int x = from.index.x + dx;
int y = from.index.y + dy;
int z = from.index.z + dz;
float distX = x - start.x;
float distY = y - start.y;
float distZ = z - start.z;
float sqrDist = distX * distX + distY * distY + distZ * distZ;
if (sqrDist > sqrMaxDistance) {
resultMoves[dx + 1][dy + 1][dz + 1] = 0;
}
}
}
}
}
return resultMoves;
}
}