fork download
copy 
  1. /* package whatever; // don't place package name! */
  2.  
  3. import java.util.*;
  4. import java.lang.*;
  5. import java.io.*;
  6.  
  7. /* Name of the class has to be "Main" only if the class is public. */
  8. class Ideone
  9. {
  10.     static double heuristic( int dx, int dy ) {
  11.         return dx + dy;
  12.     }
  13.  
  14.     static class INode {
  15.  
  16.         int x;
  17.         int y;
  18.  
  19.         boolean walkable = true;
  20.  
  21.         private double g;
  22.         private double h;
  23.  
  24.         private boolean closed;
  25.         private boolean open;
  26.  
  27.  
  28.         private INode parent;
  29.  
  30.         public INode(int x, int y) {
  31.             this.x = x;
  32.             this.y = y;
  33.         }
  34.  
  35.         public void setG(double g) {
  36.             this.g = g;
  37.         }
  38.  
  39.         public double getG() {
  40.             return g;
  41.         }
  42.  
  43.         public void setH(double h) {
  44.             this.h = h;
  45.         }
  46.  
  47.         public double getH() {
  48.             return h;
  49.         }
  50.  
  51.         public int getX() {
  52.             return x;
  53.         }
  54.  
  55.         public int getY() {
  56.             return y;
  57.         }
  58.  
  59.         public boolean isClosed() {
  60.             return closed;
  61.         }
  62.  
  63.         public void close() {
  64.             closed = true;
  65.         }
  66.  
  67.         public boolean isOpened() {
  68.             return open;
  69.         }
  70.  
  71.         public void open() {
  72.             open = true;
  73.         }
  74.  
  75.         public void setParent(INode node) {
  76.             this.parent = node;
  77.         }
  78.     }
  79.  
  80.     static class Grid {
  81.  
  82.         final int SIZE = 32;
  83.  
  84.         INode[][] nodes = new INode[SIZE][SIZE];
  85.  
  86.         {
  87.             getNode(5, 5).walkable = false;
  88.             getNode(5, 6).walkable = false;
  89.             getNode(6, 5).walkable = false;
  90.         }
  91.  
  92.         boolean valid( int x, int y ) {
  93.             return 0 <= x && x < SIZE && 0 <= y && y < SIZE;
  94.         }
  95.  
  96.         public INode getNode(int x, int y) {
  97.             return nodes[x][y] != null ? nodes[x][y] : (nodes[x][y] = new INode(x, y));
  98.         }
  99.  
  100.         public INode[] getNeighbors(int x, int y) {
  101.             List<INode> result = new ArrayList<>(8);
  102.             for ( int ix = x - 1; ix <= x + 1; ix += 1 ) {
  103.                 for ( int iy = y - 1; iy <= y + 1; iy += 1 ) {
  104.                     if ( !(ix == x && iy == y) && valid( ix, iy ) && getNode( ix, iy ).walkable ) {
  105.                         result.add( getNode( ix, iy ) );
  106.                     }
  107.                 }
  108.             }
  109.             return result.toArray( new INode[0] );
  110.         }
  111.     }
  112.  
  113.     static Grid grid = new Grid();
  114.  
  115.     static INode findMinF( List<INode> nodes ) {
  116.         return nodes.stream().min( Comparator.comparing( node -> node.getG() + node.getH() ) ).orElseThrow( RuntimeException::new );
  117.     }
  118.  
  119.     private static double Weight = 1d;
  120.  
  121.     static class Util {
  122.  
  123.         public static int[][] backtrace(INode node) {
  124.             List<int[]> result = new ArrayList<>();
  125.             INode p = node;
  126.             while ( p != null ) {
  127.                 result.add( new int[] {p.getX(), p.getY()} );
  128.                 p = p.parent;
  129.             }
  130.  
  131.             return result.toArray( new int[0][] );
  132.         }
  133.  
  134.     }
  135.  
  136.     public static int[][] findPath( int startX, int startY, int endX, int endY) {
  137.     ArrayList<INode> openList = new ArrayList<INode>();
  138.  
  139.     INode startNode = grid.getNode(startX, startY);
  140.     double SQRT2 = Math.sqrt(2);
  141.     INode node, neighbor;
  142.     INode[] neighbors;
  143.     int x, y;
  144.     double ng;
  145.  
  146.     startNode.setG(0);
  147.     startNode.setH( heuristic( Math.abs(startX - endX), Math.abs(startY - endY) ) );
  148.  
  149.     openList.add(startNode);
  150.     startNode.open();
  151.  
  152.     while ( !openList.isEmpty() ) {
  153.  
  154.         node = findMinF( openList );
  155.         openList.remove(node);
  156.         node.close();
  157.  
  158.         if ( node.getX() == endX && node.getY() == endY ) {
  159.             //Success
  160.             return Util.backtrace(node);
  161.         }
  162.  
  163.         neighbors = grid.getNeighbors( node.getX(), node.getY() );
  164.  
  165.         for ( int i = 0; i < neighbors.length; i++ ) {
  166.  
  167.             neighbor = neighbors[i];
  168.  
  169.             if ( neighbor.isClosed() ) {
  170.                 continue;
  171.             }
  172.  
  173.             x = neighbor.getX();
  174.             y = neighbor.getY();
  175.  
  176.             ng = node.getG() + ( ((x - node.getX())&(y - node.getY())) == 0 ? 1.0 : SQRT2);
  177.  
  178.             if ( !neighbor.isOpened() || ng < neighbor.getG() ) {
  179.  
  180.                 neighbor.setG( ng );
  181.                 neighbor.setH( Weight * heuristic(Math.abs(x - endX), Math.abs(y - endY)) );
  182.                 neighbor.setParent( node );
  183.  
  184.             }
  185.  
  186.             if ( !neighbor.isOpened() ) {
  187.  
  188.                 openList.add(neighbor);
  189.                 neighbor.open();
  190.  
  191.             }
  192.         }
  193.  
  194.  
  195.     }
  196.     //fail 
  197.     System.out.println("fail");
  198.     return new int[0][0];}
  199.  
  200.     public static void main(String[] args) {
  201.         int[][] path = findPath( 1, 1, 10, 10 );
  202.  
  203.         for ( int[] dot : path ) {
  204.             System.out.printf( "[%2d, %2d]%n", dot[0], dot[1] );
  205.         }
  206.  
  207.     }
  208. }
Success #stdin #stdout 0.17s 2184192KB
comments (?)
stdin
copy 
Standard input is empty
stdout
copy 
[10, 10]
[ 9, 10]
[ 8, 10]
[ 7,  9]
[ 6,  8]
[ 5,  7]
[ 4,  6]
[ 4,  5]
[ 4,  4]
[ 3,  3]
[ 2,  2]
[ 1,  1]
https://ideone.com/cgfBlc
language:
Java (HotSpot 12)
created:
5 years ago
visibility:
public

Share or Embed source code

Discover > Sphere Engine API

The brand new service which powers Ideone!

Discover > IDE Widget

Widget for compiling and running the source code in a web browser!