So I am trying to construct a 3D KD Tree from a list of randomly generated points. I am trying to accomplish this task recursively as well. But in my recursion I am facing an error when I'm trying to partition my list of points. My code is as follows:
public class Scratch {
/**
* #param args the command line arguments
*/
public static void main(String[] args) {
ArrayList<Point> points = new ArrayList<Point>();
Random rand = new Random(System.currentTimeMillis());
for (int i = 0; i < 100; i++) {
double x = rand.nextInt(100);
double y = rand.nextInt(100);
double z = rand.nextInt(100);
Point point = new Point(x, y, z);
points.add(point);
}
Node root = kdtree(points, 0);
System.out.println("Done");
}
static class Node {
Node leftChild;
Node rightChild;
Point location;
Node() {
}
Node(Node leftChild, Node rightChild, Point location) {
this.leftChild = leftChild;
this.rightChild = rightChild;
this.location = location;
}
}
public static Node kdtree(ArrayList<Point> points, int depth) {
int axis = depth % 3;
switch (axis) {
case 0:
Collections.sort(points, Point.X_COMPARATOR);
break;
case 1:
Collections.sort(points, Point.Y_COMPARATOR);
break;
case 2:
Collections.sort(points, Point.Z_COMPARATOR);
break;
default:
break;
}
int middle = points.size() / 2;
Point median = points.get(middle);
ArrayList<Point> greater = new ArrayList<Point>(points.subList(0, (middle - 1)));
ArrayList<Point> lesser = new ArrayList<Point>(points.subList((middle + 1), (points.size())));
Node node = new Node();
node.location = median;
if(greater.size() == 0 || lesser.size() == 0) {
node.leftChild = null;
node.rightChild = null;
} else {
node.leftChild = kdtree(lesser, depth + 1);
node.rightChild = kdtree(greater, depth + 1);
}
return node;
}
}
The class point is a simple object which contains an x, y and z coordinate. And three comparators used based on the depth of the tree.
The error I am getting is as follows:
Exception in thread "main" java.lang.IllegalArgumentException: fromIndex(0) > toIndex(-1)
at java.util.ArrayList.subListRangeCheck(ArrayList.java:1006)
at java.util.ArrayList.subList(ArrayList.java:996)
at scratch.Scratch.kdtree(Scratch.java:71)
at scratch.Scratch.kdtree(Scratch.java:80)
at scratch.Scratch.kdtree(Scratch.java:79)
at scratch.Scratch.kdtree(Scratch.java:79)
at scratch.Scratch.kdtree(Scratch.java:79)
at scratch.Scratch.kdtree(Scratch.java:79)
at scratch.Scratch.main(Scratch.java:32)
Im gonna go on a guess here and say that if you have only one point (or 0) on the points array, when you do middle=points.size/2 that equals 0 (integer division) and then when you try to make a sublist from 0 to -1 it throws that Exception.
Related
I'm creating a 2d top down game where the enemy AI is constantly following the player and avoiding obstacles. I did some research about path-finding algorithms and I decided to implement the breadth first search, but for some reason, the xy coordinates of the path are reversed, even though the grid is correct.
Code:
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
public class BFSTest {
// 1 = normal node
// 0 = obstacle
// S = start
// D = destination
private static char[][] nodes = {
{'S', '1', '1', '1'},
{'0', '0', '0', '1'},
{'0', '0', '0', '1'},
{'1', '1', '1', 'D'}
};
public static void main(String[] args) {
shortestPath();
}
public static List<Node> shortestPath() {
// key node, value parent
Map<Node, Node> parents = new HashMap<Node, Node>();
Node start = null;
Node end = null;
// find the start node
for (int row = 0; row < nodes.length; row++) {
for (int column = 0; column < nodes[row].length; column++) {
if (nodes[row][column] == 'S') {
start = new Node(row, column, nodes[row][column]);
break;
}
}
}
if (start == null) {
throw new RuntimeException("can't find start node");
}
// traverse every node using breadth first search until reaching the destination
List<Node> temp = new ArrayList<Node>();
temp.add(start);
parents.put(start, null);
boolean reachDestination = false;
while (temp.size() > 0 && !reachDestination) {
Node currentNode = temp.remove(0);
List<Node> children = getChildren(currentNode);
for (Node child : children) {
// Node can only be visited once
if (!parents.containsKey(child)) {
parents.put(child, currentNode);
char value = child.getValue();
if (value == '1') {
temp.add(child);
} else if (value == 'D') {
temp.add(child);
reachDestination = true;
end = child;
break;
}
}
}
}
if (end == null) {
throw new RuntimeException("can't find end node");
}
// get the shortest path
Node node = end;
List<Node> path = new ArrayList<Node>();
while (node != null) {
path.add(0, node);
node = parents.get(node);
}
printPath(path);
return path;
}
private static List<Node> getChildren(Node parent) {
List<Node> children = new ArrayList<Node>();
int x = parent.getX();
int y = parent.getY();
if (x - 1 >= 0) {
Node child = new Node(x - 1, y, nodes[x - 1][y]);
children.add(child);
}
if (y - 1 >= 0) {
Node child = new Node(x, y - 1, nodes[x][y - 1]);
children.add(child);
}
if (x + 1 < nodes.length) {
Node child = new Node(x + 1, y, nodes[x + 1][y]);
children.add(child);
}
if (y + 1 < nodes[0].length) {
Node child = new Node(x, y + 1, nodes[x][y + 1]);
children.add(child);
}
return children;
}
private static void printPath(List<Node> path) {
for (int row = 0; row < nodes.length; row++) {
for (int column = 0; column < nodes[row].length; column++) {
String value = nodes[row][column] + "";
// mark path with X
for (int i = 1; i < path.size() - 1; i++) {
Node node = path.get(i);
if (node.getX() == row && node.getY() == column) {
value = "X";
break;
}
}
if (column == nodes[row].length - 1) {
System.out.println(value);
} else {
System.out.print(value + " ");
}
}
}
System.out.println("Path: " + path);
}
}
class Node {
private int x;
private int y;
private char value;
public Node(int x, int y, char value) {
this.x = x;
this.y = y;
this.value = value;
}
public int getX() {
return x;
}
public int getY() {
return y;
}
public char getValue() {
return value;
}
#Override
public String toString() {
return "(x: " + x + " y: " + y + ")";
}
#Override
public int hashCode() {
return x * y;
}
#Override
public boolean equals(Object o) {
if (this == o) return true;
if (o == null) return false;
if (this.getClass() != o.getClass()) return false;
Node node = (Node) o;
return x == node.x && y == node.y;
}
/* Output:
* S X X X
* 0 0 0 X
* 0 0 0 X
* 1 1 1 D
* Path: [(x: 0 y: 0), (x: 0 y: 1), (x: 0 y: 2), (x: 0 y: 3), (x: 1 y: 3), (x: 2 y: 3), (x: 3 y: 3)]
*/
}
Thanks!
Think about this for a moment. Remember that your first array index is row, and the second is column. Now, in terms of conventional x and y:
the y index always refers to the row (because you count rows vertically);
the x index always refers to the column (because you count columns horizontally).
So, the correct way to index your grid is nodes[y][x]
How do I compute the distance from a start vertex to all other vertices using BFS search?
If there is no path to a vertex then the distance should be reported as -1.
I have a class that generates a Graph and a method distance(int start) that I have implemented BFS search on, but I do not not how to compute the distances and return it in a suitable data-structure.
Ecpected output:
graph.distance(0);
>>> Distance from vertex 0 to 1 is 1
>>> Distance from vertex 0 to 2 is 1
>>> Distance from vertex 0 to 3 is 2
>>> Distance from vertex 0 to 4 is 2
>>> Distance from vertex 0 to 5 is 3
import java.util.*;
import static java.lang.System.out;
import java.util.concurrent.ThreadLocalRandom;
public class Graph {
int _vertices;
ArrayList<ArrayList<Integer>> adj_list;
private void addEdge(int u, int v) {
adj_list.get(u).add(v);
adj_list.get(v).add(u);
//out.println(adj_list);
}
/*
* loop through all pairs of vertices u, v and decide,
* randomly with probability p, whether the edge (u, v)
* is in the graph.
*/
Graph(int vertices, double prob){
_vertices = vertices;
adj_list = new ArrayList<ArrayList<Integer>>(vertices);
for (int u = 0; u < vertices; u++) {
//System.out.println(i);
adj_list.add(new ArrayList<Integer>());
}
for (int v = 0; v < vertices; v++) {
for(int u = 0; u < vertices; u++) {
double random = ThreadLocalRandom.current().nextDouble(0, 1);
if (random > prob) {
//System.out.println(checkElem(adj_list, v));
if (checkElem(adj_list, v, u) == false && u != v){
addEdge(v, u);
}
}
}
}
}
public void printGraph() {
for (int i = 0; i < adj_list.size(); i++) {
System.out.println("\nAdjacency list of vertex " + i);
for (int j = 0; j < adj_list.get(i).size(); j++) {
System.out.print(" -> "+adj_list.get(i).get(j));
}
System.out.println();
}
}
/*
* #param vert: A vertex in the graph
*/
public void printVertex(int vert) {
System.out.print(" -> "+adj_list.get(vert));
}
/*
* #param arr: list of list that represents graph
* #param vertex: a vertex in the graph
* #param node: node to be checked in vertex
*/
private boolean checkElem(ArrayList<ArrayList<Integer>> arr, int vertex, int node) {
ArrayList<Integer> temp = arr.get(vertex);
if(temp.contains(node)){
return true;
} else {
return false;
}
}
/*
* #param start: A vertex to start the search from in the graph
*/
public void distance(int start) {
boolean visited[] = new boolean[_vertices];
ArrayList<Integer> queue = new ArrayList<Integer>();
visited[start] = true;
queue.add(start);
while (queue.size() != 0) {
//out.println(queue);
// Dequeue a vertex from queue and print it
start = queue.remove(0);
// Get all adjacent vertices of the dequeued vertex s
// If a adjacent has not been visited, then mark it
// visited and enqueue it
ArrayList<Integer> temp = adj_list.get(start);
Iterator<Integer> i = temp.listIterator();
//out.println("Vertex: " + start +" Dist: " + edgeDist);
while (i.hasNext()) {
out.println(start);
int n = i.next();
if (!visited[n]) {
visited[n] = true;
queue.add(n);
}
}
}
}
public static void main(String[] args) {
Graph graph = new Graph(5, 0.5);
graph.distance(0);
}
}
Calculating the distance from the source to all adjacency
Update your code to use isEmpty() because it's constant time and don't use size()==0
, Use Queue to add adjacency vertex
public int distance(int vertex) {
boolean visited[] = new boolean[_vertices];
Queue<Integer> queue = new ArrayDeque<>();
visited[vertex] = true;
queue.add(vertex);
int distance = 0;
while (!queue.isEmpty()) {
int v = queue.poll();
List<Integer> adj = adj_list.get(v);
distance++;
for (Integer w : adj) {
if (!visited[w]) {
System.out.println("Distance from vertex: " + vertex + " to: " + w +" is " + distance);
visited[w] = true;
queue.add(w);
}
}
}
return distance == 0 ? -1 : distance;
}
I am trying to implement min-heap which includes methods like insert,delete and heap sort.I am using implementation of max-heap and trying to convert it to min-heap.But,i am having some minor issues.It's a very straight-forward method ,but i am missing something,which i am not able to get.
This is the Helper Max-heap implementation i am using:
public void trickleDown(int index)
{
int largerChild;
Node top = heapArray[index]; // save root
while(index < currentSize/2) // while node has at
{ // least one child,
int leftChild = 2*index+1;
int rightChild = leftChild+1;
// find larger child
if( rightChild < currentSize && // (rightChild exists?)
heapArray[leftChild].getKey() <
heapArray[rightChild].getKey() )
largerChild = rightChild;
else
largerChild = leftChild;
// top >= largerChild?
if(top.getKey() >= heapArray[largerChild].getKey())
break;
// shift child up
heapArray[index] = heapArray[largerChild];
index = largerChild; // go down
} // end while
heapArray[index] = top; // index <- root
} // end trickleDown()
/////// My Implementation
/** Removes the top element of the heap and returns it.
*
* Complexity: O(log n)
* #return Top (min/max) element of the heap.
* #throws IllegalStateException if the heap is empty.
*/
T remove() {
if (size == 0) {
throw new IllegalStateException();
}
Comparable root = data[0];
data[0] = data[size-1];
size--;
trickleDown(0);
return (T) root;
}
private void trickleDown(int i) {
int largerChild;
Comparable top = data[i]; // save root
while(i > size/2 ) // not on bottom row{
int leftChild = left(i);
int rightChild = right(i);
if(rightChild > size && data[left(i)].compareTo(data[right(i)]) < 0 )
largerChild = leftChild;
else
largerChild = rightChild;
if(data[i].compareTo(data[right(i)]) <= 0 )
break;
data[i] = data[largerChild];
i = largerChild;
}
data[i] = top;
}
///// Test File
void checkHeapOrder(MinHeap h) {
assertTrue(h != null);
for(int i = 1; i < h.size() / 2; ++i)
assertTrue("Heap order property is broken at element at position "
+ i,
h.data[i].compareTo(h.data[i*2]) < 0 &&
h.data[i].compareTo(h.data[i*2 + 1]) < 0);
}
#Test
public void testRemove() {
System.out.println("remove");
MinHeap h = new MinHeap(10);
boolean throws_exception = false;
try {
h.remove();
} catch (IllegalStateException e) {
throws_exception = true;
} catch (Throwable e) {
}
assertTrue("remove throws an exception when empty", throws_exception);
// Permutation of 0...9
int[] input = { 0, 5, 9, 2, 3, 1, 6, 8, 7, 4 };
for(int i : input)
h.insert(i);
assertTrue(h.isFull());
for(int i = 10; i > 0; --i) {
assertEquals(h.size(), i);
Integer x = (Integer)h.remove();
assertEquals(x, new Integer(10-i)); // Items are removed in correct order
checkHeapOrder(h);
}
testRemove Failed: expected<0> but was <1>
I am pretty sure that the code is simple and i have tried to change everything from max to min,but just missing on with something,which i am having a hard time figuring out.
Background: I am making a 2D moba game and i need pathfind for all the monsters in the game. I want to give a startPos and a endPos and have the monster travel there avoiding objects.
Question:
I have been trying to implement path finding into my game for awhile now and i just cant see to get it working. All i want is some method/class where i can give it a 2d array of values (ie. true = occupied & false = free), startPos, endPos and it gives me a list of moves to get to the end. All my implementations have failed thus far. Can anyone help by giving me code that is easy to implement?
Note:
So far i have tried implementing A and it either ignored walls or sent the character into a completely random direction.
*I did get it working but in a ugly and wrong way. I had the charater move forward until it hit and wall. Then it turned right and kept moving until it could turn left and continue towards the destination. This works but i dont think people want their teams monsters running around on walls
Edit:
Code below is now working! I found that for some reason the points were backwards so i had to invert the Point list. No all i need to do is interpolate between points to give smooth movement. However, I do ask is there any way i cant add more bias towards walls. For example making it so the point never goes within 1 unit of a wall?
package NavMesh;
import java.awt.Point;
import java.util.ArrayList;
import java.util.List;
import toolbox.Maths;
public class MovementPath {
private Node[][] mapOriginal;
private Node[][] mapPath;
public boolean solving = true;
public int startX, startY, finishX, finishY, cells;
private int checks = 0;
private int length = 0;
int realStartX, realStartY, realFinishX, realFinishY;
NavMesh mesh;
private Algorithm alg;
List<Point> path = new ArrayList<Point>();
public MovementPath(NavMesh mesh,int startX, int startY, int finishX, int finishY) {
this.mapOriginal = mesh.getMapCopy();
this.mesh = mesh;
this.startX = startX;
this.startY = startY;
this.finishX = finishX;
this.finishY = finishY;
this.cells = mapOriginal.length;
realStartX = startX;
realStartY = startY;
realFinishX = finishX;
realFinishY = finishY;
this.startX = (int) (Math.floor((float) startX / (float) mesh.cellWidth));
this.startY = (int) (Math.floor((float) startY / (float) mesh.cellHeight));
this.finishX = (int) (Math.floor((float) finishX / (float) mesh.cellWidth));
this.finishY = (int) (Math.floor((float) finishY / (float) mesh.cellHeight));
mapPath = new Node[mapOriginal.length][mapOriginal.length];
System.arraycopy(mapOriginal, 0, mapPath, 0, mapOriginal.length);
mapPath[this.startX][this.startY] = new Node(0,this.startX,this.startY);;
mapPath[this.finishX][this.finishY] = new Node(1,this.finishX,this.finishY);
addPointCentered(realFinishX,realFinishY);
alg = new Algorithm();
//alg.AStar();
alg.Dijkstra();
addPointCentered(realStartX,realStartY);
mesh.drawMap(Integer.toString(Maths.randomRange(0, 1000)), mapPath);
}
public Path getPath(){
//System.out.println("Returning path with " + getPathPoints().size() + " points");
return new Path(getPathPoints());
}
private void addPointCentered(int x, int y) {
path.add(new Point(x+(mesh.cellWidth/2),y+(mesh.cellHeight/2)));
}
public List<Point> getPathPoints(){
List<Point> rPath = new ArrayList<Point>();
for(int i = path.size()-1; i >= 0; i--) {
rPath.add(path.get(i));
}
return rPath;
}
class Algorithm { //ALGORITHM CLASS
//A STAR WORKS ESSENTIALLY THE SAME AS DIJKSTRA CREATING A PRIORITY QUE AND PROPAGATING OUTWARDS UNTIL IT FINDS THE END
//HOWEVER ASTAR BUILDS IN A HEURISTIC OF DISTANCE FROM ANY NODE TO THE FINISH
//THIS MEANS THAT NODES THAT ARE CLOSER TO THE FINISH WILL BE EXPLORED FIRST
//THIS HEURISTIC IS BUILT IN BY SORTING THE QUE ACCORDING TO HOPS PLUS DISTANCE UNTIL THE FINISH
public void AStar() {
ArrayList<Node> priority = new ArrayList<Node>();
priority.add(mapPath[startX][startY]);
while(solving) {
if(priority.size() <= 0) {
solving = false;
break;
}
int hops = priority.get(0).getHops()+1;
ArrayList<Node> explored = exploreNeighbors(priority.get(0),hops);
if(explored.size() > 0) {
priority.remove(0);
priority.addAll(explored);
} else {
priority.remove(0);
}
sortQue(priority); //SORT THE PRIORITY QUE
}
}
public void Dijkstra() {
ArrayList<Node> priority = new ArrayList<Node>(); //CREATE A PRIORITY QUE
priority.add(mapPath[startX][startY]); //ADD THE START TO THE QUE
while(solving) {
if(priority.size() <= 0) { //IF THE QUE IS 0 THEN NO PATH CAN BE FOUND
solving = false;
break;
}
int hops = priority.get(0).getHops()+1; //INCREMENT THE HOPS VARIABLE
ArrayList<Node> explored = exploreNeighbors(priority.get(0), hops); //CREATE AN ARRAYLIST OF NODES THAT WERE EXPLORED
if(explored.size() > 0) {
priority.remove(0); //REMOVE THE NODE FROM THE QUE
priority.addAll(explored); //ADD ALL THE NEW NODES TO THE QUE
} else { //IF NO NODES WERE EXPLORED THEN JUST REMOVE THE NODE FROM THE QUE
priority.remove(0);
}
}
}
public ArrayList<Node> sortQue(ArrayList<Node> sort) { //SORT PRIORITY QUE
int c = 0;
while(c < sort.size()) {
int sm = c;
for(int i = c+1; i < sort.size(); i++) {
if(sort.get(i).getEuclidDist(finishX,finishY)+sort.get(i).getHops() < sort.get(sm).getEuclidDist(finishX,finishY)+sort.get(sm).getHops())
sm = i;
}
if(c != sm) {
Node temp = sort.get(c);
sort.set(c, sort.get(sm));
sort.set(sm, temp);
}
c++;
}
return sort;
}
/*
public ArrayList<Node> exploreNeighbors(Node current, int hops) { //EXPLORE NEIGHBORS
ArrayList<Node> explored = new ArrayList<Node>(); //LIST OF NODES THAT HAVE BEEN EXPLORED
for(int a = -1; a <= 1; a++) {
for(int b = -1; b <= 1; b++) {
int xbound = current.getX()+a;
int ybound = current.getY()+b;
if((xbound > -1 && xbound < cells) && (ybound > -1 && ybound < cells)) { //MAKES SURE THE NODE IS NOT OUTSIDE THE GRID
Node neighbor = mapPath[xbound][ybound];
if((neighbor.getHops()==-1 || neighbor.getHops() > hops) && neighbor.getType()!=2) { //CHECKS IF THE NODE IS NOT A WALL AND THAT IT HAS NOT BEEN EXPLORED
explore(neighbor, current.getX(), current.getY(), hops); //EXPLORE THE NODE
explored.add(neighbor); //ADD THE NODE TO THE LIST
}
}
}
}
return explored;
}
*/
public ArrayList<Node> exploreNeighbors(Node current, int hops) { //EXPLORE NEIGHBORS
ArrayList<Node> explored = new ArrayList<Node>(); //LIST OF NODES THAT HAVE BEEN EXPLORED
//test(hops, current, explored,current.getX(),current.getY());
//test(hops, current, explored,current.getX()+1,current.getY());
//test(hops, current, explored,current.getX()-1,current.getY());
//test(hops, current, explored,current.getX(),current.getY()+1);
//test(hops, current, explored,current.getX(),current.getY()-1);
for(int a = -1; a <= 1; a++) {
for(int b = -1; b <= 1; b++) {
test(hops, current, explored,current.getX()+a,current.getY()+b);
}
}
return explored;
}
private void test(int hops, Node current, ArrayList<Node> explored, int xbound, int ybound) {
if((xbound > -1 && xbound < cells) && (ybound > -1 && ybound < cells)) { //MAKES SURE THE NODE IS NOT OUTSIDE THE GRID
Node neighbor = mapPath[xbound][ybound];
if((neighbor.getHops()==-1 || neighbor.getHops() > hops) && neighbor.getType()!=2) { //CHECKS IF THE NODE IS NOT A WALL AND THAT IT HAS NOT BEEN EXPLORED
explore(neighbor, current.getX(), current.getY(), hops); //EXPLORE THE NODE
explored.add(neighbor); //ADD THE NODE TO THE LIST
}
}
}
public void explore(Node current, int lastx, int lasty, int hops) { //EXPLORE A NODE
if(current.getType()!=0 && current.getType() != 1) //CHECK THAT THE NODE IS NOT THE START OR FINISH
current.setType(4); //SET IT TO EXPLORED
current.setLastNode(lastx, lasty); //KEEP TRACK OF THE NODE THAT THIS NODE IS EXPLORED FROM
current.setHops(hops); //SET THE HOPS FROM THE START
checks++;
if(current.getType() == 1) { //IF THE NODE IS THE FINISH THEN BACKTRACK TO GET THE PATH
backtrack(current.getLastX(), current.getLastY(),hops);
}
}
public void backtrack(int lx, int ly, int hops) { //BACKTRACK
length = hops;
while(hops > 1) { //BACKTRACK FROM THE END OF THE PATH TO THE START
Node current = mapPath[lx][ly];
current.setType(5);
addPointCentered(lx*mesh.cellWidth,ly*mesh.cellHeight);
//System.out.println("New Point: " + path.get(path.size()-1).toString());
lx = current.getLastX();
ly = current.getLastY();
hops--;
}
solving = false;
}
}
}
Try A*, I used that for a path finding problem. It is easy to implement for grid based movement and very fast. I implemented it using the pseudocode on the wikipedia page.
My problem is that the movement cost(G cost) of my node and heuristic is inaccurate it does not match with the picture.
Here is the image of what I'm following.There are three labels here and the movement cost is labelled at the bottom left and the heuristic is at bottom right. Label at top-left is the F = H + G
Here is my output. As you can see the movement cost is not the same as the desired output. The red circle is the goal node.
Also the same with my Heuristic cost.
public class AStarPathFinder implements PathFinder {
private List<Node> open = new ArrayList<Node>();
private List<Node> close = new ArrayList<Node>();
private Node[][] nodes;
private TileMap map;
private Heuristic heuristic;
public AStarPathFinder(TiledMapStage mapStage, Heuristic heuristic) {
this.heuristic = heuristic;
nodes = mapStage.getNodes();
map = mapStage.getMap();
}
#Override
public Path findPath(int startX, int startY, int goalX, int goalY) {
clearNodes();
Node goal = nodes[goalX][goalY];
Node current = nodes[startX][startY];
open.add(current);
while (!open.isEmpty()) {
current = getLowestFcost(open);
open.remove(current);
close.add(current);
if (current == goal) {
Path path = new Path();
while (current != null) {
path.add(current);
current = current.parent;
}
return path;
}
// neighbors of current
for (int x = -1; x < 2; x++) {
for (int y = -1; y < 2; y++) {
int dx = current.x + x;
int dy = current.y + y;
if (map.isValidLocation(dx, dy)) {
if (!map.isWalkable(nodes[dx][dy], x, y) || close.contains(nodes[dx][dy]))
continue;
float newScore = movementCost(current.g, isDiagonal(x, y));
if (!open.contains(nodes[dx][dy])) {
open.add(nodes[dx][dy]);
} else if (newScore >= nodes[dx][dy].g) continue;
nodes[dx][dy].g = newScore;
nodes[dx][dy].h = heuristic.estimate(nodes[dx][dy], goal);
nodes[dx][dy].f = nodes[dx][dy].g + nodes[dx][dy].h;
nodes[dx][dy].parent = current;
nodes[dx][dy].label.setText((int) nodes[dx][dy].g + "");
}
}
}
}
return null;
}
private Node getLowestFcost(List<Node> open) {
Node lowestNode = open.get(0);
for (int i = 0; i < open.size(); i++) {
if (open.get(i).f <= lowestNode.f && open.get(i).h < lowestNode.h) {
lowestNode = open.get(i);
}
}
return lowestNode;
}
private boolean isDiagonal(int x, int y) {
return (x == -1 && y == 1 ||
x == 1 && y == 1 ||
x == 1 && y == -1 ||
x == -1 && y == -1);
}
private float movementCost(float cost, boolean diagonal) {
return diagonal ? cost + 14 : cost + 10;
}
#Override
public void clearNodes() {
for (int i = 0; i < map.getTileWidth(); i++) {
for (int j = 0; j < map.getTileHeight(); j++) {
if (nodes[i][j].cell != null) {
nodes[i][j].label.setText("");
nodes[i][j].f = 0;
nodes[i][j].h = 0;
nodes[i][j].g = 0;
nodes[i][j].arrow.setDrawable("cursor");
nodes[i][j].arrow.setVisible(false);
nodes[i][j].parent = null;
}
}
}
close.clear();
open.clear();
}
}
Here is the pseudocode that I'm following. Also my heuristic is a diagonal distance
It looks like your problem is in the isWalkable method of your TileMap map variable.
The image you're following doesn't allow to pass diagonally alongside a wall, where your algorithm does.
You can see this because the score gets added with 14 as follows: 14 + 14 = 28. While you expected it to go as follows: 14 + 10 (going down first) + 10 (going right) = 34.
I hope I explained your problem clearly. I don't know your implementation of isWalkable, so I can't provide a full solution but I hope I have pointed you in the right direction.