Thuật toán A sao tìm đường đi

Chương trình này mình tự viết theo thuật toán tìm được trên mạng.

Chương trình còn nhiều sai sót vì chưa có nhiều thời gian sửa chữa

Tải file chạy tại đây

Một phần của đoạn code:

 /*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package pathfinding.finder.d10cn5.nhom9;

import java.util.ArrayList;
import java.util.LinkedList;
import java.util.List;
import pathfinding.enviroment.PCell;
import pathfinding.enviroment.PDirection;
import pathfinding.enviroment.PGrid;
import pathfinding.finder.AbstractPFinder;

/**
 *
 * @author Administrator
 */
public class ThuatToanASao extends AbstractPFinder{
     private ArrayList<PState> O;
    private ArrayList<PState> C;

    public ThuatToanASao(PGrid grid) {
        // set grid
        super(grid);
        // Initialised State Queue
        O = new ArrayList();
        C = new ArrayList();

    }

    @Override
    public List<PDirection> find() {
        // Get the start stage
        O.clear();
        C.clear();
        O.add(new PState(mGrid.getStart(), new LinkedList<PDirection>()));

        double f = mGrid.heuristicA(O.get(0).getPCell());
        O.get(0).setf(f);
        O.get(0).setg(0);

        while (!O.isEmpty()) { //Nếu hàng đợi không trống

            //Duyet danh sach O
            int tmp = 0;
            for (int i = 0; i < O.size(); ++i) {
                if (O.get(tmp).getf() > O.get(i).getf()) {
                    tmp = i;
                }
            }

            // Get current
            PState current = O.remove(tmp);

            C.add(current);

            // Check if gold
            if (mGrid.isGold(current.getPCell())) {

                return current.getPath();
            }

            List<PState> nexts = getNextState(current);
            for (int i = 0; i < nexts.size(); ++i) {

                if (C.contains(nexts.get(i)) == true) {
                    continue;
                }

                nexts.get(i).setg(current.getg() + mGrid.getCost(nexts.get(i).getPCell()));
                if (nexts.get(i).getg() < current.getg()) {
                    continue;
                }
                if (O.contains(nexts.get(i)) == false) {

                    f = mGrid.heuristicA(nexts.get(i).getPCell());

                    nexts.get(i).setf(f + nexts.get(i).getg());


                    O.add(nexts.get(i));
                }
            }

        }

        return null;
    }

    private List<PState> getNextState(PState current) {
        List<PState> nexts = new LinkedList<>();

        // Get current position
        // Get current path
        PCell cell = current.getPCell();
        List<PDirection> path = current.getPath();

        PDirection[] nextDirection;
        if (!path.isEmpty()) {
            // If there is a last direction
            PDirection lastDirection = path.get(path.size() -1);

            // According to the last direction choose only 5 remaining direction
            switch (lastDirection) {
                case N:
                    nextDirection = new PDirection[]{PDirection.W, PDirection.NW, PDirection.N, PDirection.NE, PDirection.E};
                    break;
                case NE:
                    nextDirection = new PDirection[]{PDirection.NW, PDirection.N, PDirection.NE, PDirection.E, PDirection.SE};
                    break;
                case E:
                    nextDirection = new PDirection[]{PDirection.N, PDirection.NE, PDirection.E, PDirection.SE, PDirection.S};
                    break;
                case SE:
                    nextDirection = new PDirection[]{PDirection.NE, PDirection.E, PDirection.SE, PDirection.S, PDirection.SW};
                    break;
                case S:
                    nextDirection = new PDirection[]{PDirection.E, PDirection.SE, PDirection.S, PDirection.SW, PDirection.W};
                    break;
                case SW:
                    nextDirection = new PDirection[]{PDirection.SE, PDirection.S, PDirection.SW, PDirection.W, PDirection.NW};
                    break;
                case W:
                    nextDirection = new PDirection[]{PDirection.S, PDirection.SW, PDirection.W, PDirection.NW, PDirection.N};
                    break;
                case NW:
                    nextDirection = new PDirection[]{PDirection.SW, PDirection.W, PDirection.NW, PDirection.N, PDirection.NE};
                    break;
                default:
                    // Never happen
                    nextDirection = null;
            }
        } else {
            // If no previous direction is found, try all 8
            nextDirection = new PDirection[]{PDirection.E, PDirection.SE, PDirection.S, PDirection.SW, PDirection.W, PDirection.NW, PDirection.N, PDirection.NE};
        }

        getNextState(nexts, cell, path, nextDirection);

        return nexts;
    }

    private void getNextState(List<PState> nextPStates, PCell cell, List<PDirection> path, PDirection[] directions) {
        for (PDirection direction : directions) {

            // Calculate next position
            int nextCol = direction.getNextCol(cell.getCol());
            int nextRow = direction.getNextRow(cell.getRow());

            PCell nextCell = new PCell(nextCol, nextRow);

            if (mGrid.isValidCell(nextCell)) {
                // Clone old path
                List<PDirection> nextPath = new LinkedList<>(path);

                // Add next direction
                nextPath.add(direction);

                // Add this to nextPStates
                nextPStates.add(new PState(nextCell, nextPath));
            }
        }
    }
}

 

*****