Examples with ObservationFromSubgoalPositionList com.microsoft.Malmo.Schemas.ObservationFromSubgoalPositionList used on opensource projects

Example 1 with ObservationFromSubgoalPositionList

use of com.microsoft.Malmo.Schemas.ObservationFromSubgoalPositionList in project malmo by Microsoft.

the class MazeDecoratorImplementation method findSubgoals.

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.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.
                // Will effectively increment destindex and smoothremovalindex.
                opath.remove(removalcandidateindex);
            } 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.");
    }
}