in Minecraft/src/main/java/com/microsoft/Malmo/MissionHandlers/MazeDecoratorImplementation.java [444:560]
private void findSubgoals(Cell[] grid, Cell start, Cell end)
{
System.out.println("Attempting to find subgoals...");
// Attempt to find subgoals - this represents the "smoothed" optimal path.
// It uses something akin to line-of-sight smoothing, to reduce the rectilinear path into something a bit more
// like what a human agent would use.
// First, copy the optimal path into an array:
ArrayList<Cell> opath = new ArrayList<Cell>();
Cell cur = end;
while (cur != start)
{
opath.add(0, cur);
cur = cur.predecessor;
}
opath.add(0, start);
// Now walk the path, removing any points that aren't required.
// For example, if the agent can walk from A directly to C, we can safely remove point B.
// This will help remove some of the 90 degree turns - eg instead of walking one square north, then one square east,
// the agent could just walk directly north-east.
int startindex = 0;
int removalcandidateindex = 1;
int destindex = 2;
if (opath.size() > 2)
{
// Walk the path, removing any points we can:
while (destindex != opath.size())
{
Cell smoothstart = opath.get(startindex);
Cell smoothremovalcandidate = opath.get(removalcandidateindex);
Cell smoothdest = opath.get(destindex);
// Traverse the shortest line from smoothstart to smoothdest looking for collisions.
// If there are none, we can safely remove the removal candidate.
double xa = smoothstart.x + 0.5;
double za = smoothstart.z + 0.5;
double xb = smoothdest.x + 0.5;
double zb = smoothdest.z + 0.5;
double dist = Math.sqrt((xb-xa)*(xb-xa) + (zb-za)*(zb-za));
int samplepoints = (int)Math.ceil(dist * 5);
boolean walkable = true;
for (int sample = 0; sample < samplepoints && walkable; sample++)
{
double f = (double)sample / (double)samplepoints;
double xs = xa + (xb-xa) * f;
double zs = za + (zb-za) * f;
int cellx = (int)Math.floor(xs);
int cellz = (int)Math.floor(zs);
// Is this cell blocked?
int cellindex = cellx + cellz * width;
if (cellindex < 0 || cellindex >= grid.length || grid[cellindex] == null)
walkable = false;
if (walkable && gapHeight > optimalPathHeight && !gapBlock.getBlock().getDefaultState().equals(Blocks.AIR.getDefaultState()))
{
// The "gaps" are in fact walls, so we need to be a bit more conservative with our path, since the
// player has a width of 0.4 cells. We do this in a very unsophisticated, brute-force manor by testing
// the four corner points of the square the player would occupy if he was standing centrally in the cell.
int lowerx = (int)Math.floor(xs-0.2);
int upperx = (int)Math.floor(xs+0.2);
int lowerz = (int)Math.floor(zs-0.2);
int upperz = (int)Math.floor(zs+0.2);
int[] cellsToTest = new int[4];
// Speed is not really an issue here so we don't worry about testing the same cells multiple times.
cellsToTest[0] = lowerx + lowerz * width;
cellsToTest[1] = lowerx + upperz * width;
cellsToTest[2] = upperx + lowerz * width;
cellsToTest[3] = upperx + upperz * width;
// Are these cells blocked?
for (int i = 0; i < 4 && walkable; i++)
{
int ctt = cellsToTest[i];
if (ctt < 0 || ctt >= grid.length || grid[ctt] == null)
walkable = false;
}
}
}
if (walkable)
{
// Can safely remove the candidate point - start->dest is walkable without it.
opath.remove(removalcandidateindex); // Will effectively increment destindex and smoothremovalindex.
}
else
{
// We need the candidate point, so set that as our new start index.
startindex = removalcandidateindex;
removalcandidateindex = startindex + 1;
destindex = startindex + 2;
smoothremovalcandidate.isSubgoal = true;
}
}
}
if (this.mazeParams.getAddNavigationObservations() != null)
{
// Add the subgoals to an observation producer:
this.navigator = new ObservationFromSubgoalPositionList();
int scale = this.mazeParams.getSizeAndPosition().getScale();
double y = 1 + this.optimalPathHeight + this.yOrg;
int i = 1;
for (Cell cell : opath)
{
double x = scale * (cell.x + 0.5) + this.xOrg;
double z = scale * (cell.z + 0.5) + this.zOrg;
PointWithToleranceAndDescription ptd = new PointWithToleranceAndDescription();
ptd.setTolerance(new BigDecimal(1.0));
ptd.setX(new BigDecimal(x));
ptd.setY(new BigDecimal(y));
ptd.setZ(new BigDecimal(z));
ptd.setDescription("MazeSubpoint_" + String.valueOf(i));
i++;
this.navigator.getPoint().add(ptd);
}
System.out.println("Found subgoals.");
}
}