We Keep Coding

KD-Tree points are repeating and giving me the wrong output

I (tried) implementing a KD-Tree in Processing/Java and followed the logic that I've seen in dozens of posts and the wikipedia article, but I must've done something wrong since the output looks like this:
Instead of this:
Clearly something is off since nodes are repeating and doesn't look like the Wikipedia Tree at all.
Here is my buildKDTree function:
public Node buildKDTree(List<Point> pointList, int depth){
int size = pointList.size();
int axis = depth % 2;
Node newNode = new Node();
if(pointList.size() == 1)
{
System.out.print("I am a leaf \n");
System.out.print("Size of list: " + pointList.size() + "\n");
System.out.print("Point: " + pointList.get(0) + "\n");
newNode.point = pointList.get(0);
newNode.left = null;
newNode.right = null;
System.out.print("Node Point: " + newNode.point + "\n");
return newNode;
}
if(size <= 0)
{
return null;
}
if(axis == 0)
{
//sort by x
System.out.print("Sorting by X \n");
Comparator<Point> com = new xComp();
Collections.sort(pointList, com);
}
else if(axis == 1)
{
System.out.print("Sorting by Y \n");
Comparator<Point> com = new yComp();
Collections.sort(pointList, com);
}
int median = size/2;
//System.out.print("Median is: " + points.get(median) + " \n");
List<Point> beforeMedian = pointList.subList(0, median);
List<Point> afterMedian = pointList.subList(median, size);
newNode.point = pointList.get(median);
newNode.left = buildKDTree(beforeMedian, depth + 1);
newNode.right = buildKDTree(afterMedian, depth + 1);
return newNode;
}
My KD Tree class, Node class, and Point class:
class KDTree{
Node root;
public KDTree()
{
root = null;
}
}
class Node{
Node left;
Node right;
Point point;
Node(Point _p, Node l, Node r){
point = _p;
left = l;
right = r;
}
Node(Node l, Node r){
left = l;
right = r;
}
Node(){
left = null;
right = null;
point = null;
}
Node(Point _p)
{
point = _p;
left = null;
right = null;
}
void setPoint(Point _p)
{
point = _p;
}
}
class Point {
public PVector p;
boolean isNearestNeighbor = false;
boolean isSearchLocation = false;
public Point( float x, float y ){
p = new PVector(x,y);
isNearestNeighbor = false;
isSearchLocation = false;
}
public Point(PVector _p0 ){
p = _p0;
}
}
How i'm printing the tree:
public void printLevelOrder(Node root)
{
if(root == null )
{
return;
}
Queue<Node> q =new LinkedList<Node>();
q.add(root);
while(true)
{
int nodeCount = q.size();
if(nodeCount == 0)
{
break;
}
while(nodeCount > 0)
{
Node node = q.peek();
System.out.print("("+node.point + ")");
q.remove();
if(node.left != null)
{
q.add(node.left);
}
if(node.right != null)
{
q.add(node.right);
}
if(nodeCount > 1)
{
System.out.print(", ");
}
nodeCount--;
}
System.out.println();
}
}
Any help would be appreciated! I've been looking at this for hours so maybe I'm missing something simple.

Rather than providing some hints on what is likely going wrong, I'd like to suggest some steps to solving this issue yourself:
break the buildKDTree method into several smaller methods performing much smaller functions (e.g. build leaf, sort sublists etc.)
unit test each one to make sure it does exactly what you expect
unit test the whole function using simple cases moving to more complex cases
if any of those unit tests behave in an unexpected manner, use an interactive debugger to find out what is going
if you can't understand why something is behaving in a certain way, as a question on SO about that case
finally, once everything is behaving correctly, apply to a realistic case
This may take a little longer but you'll learn a lot more in the process.

Trying to implement a 2D BST but my add and contains method are not working

I'm attempting to implement a kd tree, in the form of a 2D tree, however, my implementation is failing. Essentially, what I'm doing is at each level of the tree, an item is added based on either its x or y coordinate. After one adds the root node, the next node added is compared to the root nodes x coordinate and if it is less, it's placed on the left side of the root node, and if it is greater, on the right side of the root node. The next node after the 2nd node iist then added based on x-coordinate again as like in a BST, a maximum of 2 nodes can only be added per parent. The third node would be compared based on the y-coordinate.
I have made some strides to get my add and search (contains method to work) but to no avail. I have tried doing conditional tests to check whether or not the nodes when added should go on the left hand side of the root or other nodes (if their x-coord is smaller) my add method does not seem to be working. Here is my entire program so far.
import java.util.*;
public class TwoDTree {
/*************
* attributes
************/
TwoDTreeNode root;
/***************
* constructor
**************/
TwoDTree() {
root = null;
}
/**********
* methods
*********/
/**
* To Do: adds a new node with the given x and y coordinates to the TwoDTree
*
* #param x
* #param y
*/
public void add(int x, int y) {
TwoDTreeNode currentNode = root;
TwoDTreeNode previousNode = null;
TwoDTreeNode newNode = new TwoDTreeNode(x,y);
boolean useXCoordinate = false;
boolean useLeftSubTree = false;
while (currentNode != null) {
useLeftSubTree = useXCoordinate ? (newNode.xCoordinate < currentNode.xCoordinate) : (newNode.yCoordinate < currentNode.yCoordinate);
useXCoordinate = !useXCoordinate;
}
if (root == null) {
root = new TwoDTreeNode(x, y);
} else if (useLeftSubTree) {
previousNode.left = new TwoDTreeNode(x,y);
} else {
previousNode.right =new TwoDTreeNode(x,y);
}
}
/**
*
* #param x
* #param y
* #returns true if a node with the given x and y coordinates exist in the
* tree.
*/
public boolean contains(int x, int y) {
TwoDTreeNode currentNode = root;
TwoDTreeNode newNode = new TwoDTreeNode(x,y);
boolean useXCoordinate = false;
boolean useLeftSubTree = false;
while (currentNode != null) {
useLeftSubTree = useXCoordinate ? (newNode.xCoordinate < currentNode.xCoordinate) : (newNode.yCoordinate < currentNode.yCoordinate);
useXCoordinate = !useXCoordinate;
if ((currentNode.xCoordinate == newNode.xCoordinate) && (currentNode.yCoordinate == newNode.yCoordinate)) {
return true;
}
}
return false;
}
/**
* A method which prints a level order traversal of the tree
*/
public void levelOrderPrint() {
Queue<TwoDTreeNode> queue = new LinkedList<TwoDTreeNode>();
queue.add(root);
while (!queue.isEmpty()) {
TwoDTreeNode node = queue.poll();
System.out.print("(" + node.xCoordinate + "," + node.yCoordinate + ")");
if (node.left != null)
queue.add(node.left);
if (node.right != null)
queue.add(node.right);
}
System.out.println();
}
public static void main (String[] args) {
System.out.println("Building a tree of nodes: (30,40) (5,25) (10,12) (70,70) (50,30) (35,40)");
TwoDTree tDTree = new TwoDTree();
tDTree.add(30, 40);
tDTree.add(5, 25);
tDTree.add(10, 12);
tDTree.add(70, 70);
tDTree.add(50, 30);
tDTree.add(35, 40);
System.out.println("Level order traversal for this tree is:");
tDTree.levelOrderPrint();
System.out.println("contains(5,25) returned: " + tDTree.contains(5, 25));
System.out.println("contains (10,13) returned: " + tDTree.contains(10, 13));
System.out.println("contains (35,45) returned: " + tDTree.contains(35, 45));
}
private static class TwoDTreeNode {
/*************
* attributes
************/
int xCoordinate;
int yCoordinate;
TwoDTreeNode right;
TwoDTreeNode left;
/***************
* constructors
**************/
TwoDTreeNode(int x, int y) {
xCoordinate = x;
yCoordinate = y;
}
TwoDTreeNode(int x, int y, TwoDTreeNode leftChild, TwoDTreeNode rightChild) {
xCoordinate = x;
yCoordinate = y;
left = leftChild;
right = rightChild;
}
}
}

Finding the path though a list of vectors

I have a list of 325 nodes on a map of earth.
These nodes map out the planet with shipping routes.
I've tasked to make an method that can find the shortest route in distance from one node to another only using a path of nodes. It needs to be real time and quick as there will be possibly hundreds of boats.
I've looked into A* and Dijkstras solutions but they don't deal with corner well?
I was thinking about using the angle and the distances but I'm not sure how I would implement it.
Each node on the map is as the following object in an objectMap. Int being the node number.
The nodes and calculations were done within the loading of the app. So we have all of that.
The below class is contained in the objectMap and works nicely.
public static class Node
{
private int nodeNumber;
private Vector2 pos;
private int connectedNodes[];
private ObjectMap<Integer, Node> masterList;
private ObjectMap<Integer, Float> connectedNodeDistances;
private ObjectMap<Integer, Float> connectedNodeDegrees;
public Node(int nodeNumber, float x, float y, int connectedNodes[], ObjectMap<Integer, Node> masterList)
{
this.nodeNumber = nodeNumber;
float areaX = x / MapCreator.WORLD_SCALE;
float areaY = y / MapCreator.WORLD_SCALE;
this.pos = new Vector2(areaX / MapCreator.CURRENT_DOWN_SIZE_X, (PolyUtils.getInstance().getScreenPercentageY(1f) - (areaY / MapCreator.CURRENT_DOWN_SIZE_X)) - MapCreator.TOP_EXTRA);
this.connectedNodes = connectedNodes;
this.masterList = masterList;
this.connectedNodeDistances = new ObjectMap<Integer, Float>();
this.connectedNodeDegrees = new ObjectMap<Integer, Float>();
}
public void calculateDistances()
{
for(int eachConnectedNode : connectedNodes)
{
float angleDegree = new Vector2(this.pos).sub(masterList.get(eachConnectedNode).getPos()).angle();
connectedNodeDistances.put(eachConnectedNode, this.pos.dst(masterList.get(eachConnectedNode).getPos()));
connectedNodeDegrees.put(eachConnectedNode, angleDegree);
}
}
public int getNodeNumber() {
return nodeNumber;
}
public float getDistanceFromNode(int number)
{
return connectedNodeDistances.get(number);
}
public Vector2 getPos() {
return pos;
}
public int[] getConnectedNodes() {
return connectedNodes;
}
}
Where I'm stuck is:
public Array<Node> getFastestRoute(Vector2 startPos, Vector2 endPos, int numberOfAttempts)
{
Array<Array<Node>> potentialRoutes = new Array<Array<Node>>();
int sizeOfShortestRouteNodes = 999999;
for(int index = 0; index < numberOfAttempts; index++)
{
Array<Node> newRoute = getListOfNodes(startPos, endPos, MathUtils.random(-0.75f, 0.75f));
if(newRoute != null)
{
if(newRoute.size < sizeOfShortestRouteNodes)
{
potentialRoutes.clear();
potentialRoutes.add(newRoute);
sizeOfShortestRouteNodes = potentialRoutes.size;
}
else if(newRoute.size == sizeOfShortestRouteNodes)
{
potentialRoutes.add(newRoute);
}
}
}
return getShortestRouteDistance(potentialRoutes);
}
private Array<Node> getListOfNodes(Vector2 startPos, Vector2 endPos, float randomizationSeed)
{
//TODO Draw as lines as test.
Array<Node> nodeList = new Array<Node>();
Array<Node> deadNodes = new Array<Node>();
int iterations = 0;
final int maxIterations = 100; //Needs to be low so that boat trip isn't too long also helps performance.
nodeList.add(getNearestNode(startPos));
Node lastNode = getNearestNode(endPos);
Node currentNode = nodeList.first();
while(true)
{
float currentNodeToEndAngle = new Vector2(new Vector2(currentNode.getPos())).sub(endPos).angle();
//Find closest direction
Node closestNodeInDirection = null;
float closestDirection = 361;
for(int eachConnectedNode : currentNode.getConnectedNodes())
{
Node potentialNode = boatNodes.get((eachConnectedNode));
if(!deadNodes.contains(potentialNode, true))
{
float angleToEndNodeFromCurrent = (new Vector2(currentNode.getPos()).sub(potentialNode.getPos()).angle());
//Randomize the direction from the seed.
angleToEndNodeFromCurrent = angleToEndNodeFromCurrent * randomizationSeed;
float differenceInDegrees = Math.abs(angleToEndNodeFromCurrent - currentNodeToEndAngle);
if(differenceInDegrees < closestDirection)
{
closestDirection = differenceInDegrees;
closestNodeInDirection = potentialNode;
}
}
}
//No new nodes.
if(closestNodeInDirection == null)
{
//Go back and try another route.
if(nodeList.size > 1)
{
nodeList.pop();
currentNode = nodeList.peek();
}
}
//Adding nodes.
if(closestNodeInDirection != null && lastNode != closestNodeInDirection)
{
nodeList.add(closestNodeInDirection);
deadNodes.add(closestNodeInDirection);
currentNode = closestNodeInDirection;
}
else if(closestNodeInDirection != null)
{
//Last node reached.
nodeList.add(lastNode);
return nodeList;
}
//Iterations too many.
iterations++;
if(iterations >= maxIterations){
return null;
}
}
}
public Array<Node> getShortestRouteDistance(Array<Array<Node>> allNodeRoutes)
{
Array<Node> shortestRoute = null;
float shortestRouteLength = 99999f;
for(int arraysIndex = 0; arraysIndex < allNodeRoutes.size; arraysIndex++)
{
Array<Node> nodeArray = allNodeRoutes.get(arraysIndex);
float lengthOfThisRoute = 0f;
for(int nodesIndex = 0; nodesIndex < nodeArray.size; nodesIndex++)
{
Node nextNode = null;
Node thisNode = nodeArray.get(nodesIndex);
if(nodesIndex + 1 < nodeArray.size)
{
nextNode = nodeArray.get(nodesIndex + 1);
}
if(nextNode != null)
{
lengthOfThisRoute += thisNode.getDistanceFromNode(nextNode.getNodeNumber());
}
}
if(lengthOfThisRoute < shortestRouteLength)
{
shortestRouteLength = lengthOfThisRoute;
shortestRoute = nodeArray;
}
}
return shortestRoute;
}

What you are describing is a well known problem with A* and Dijkstras --- and the solution is to not use either of them.
What you need is an "any-angle" algorithm --- so you should use an algorithm called Theta* instead. This approach properly cuts near the corners while avoiding obstacles.
In fact, it is quite similar to the approach you already came up with! I recommend reading the excellent article here, it is a very good explanation of what to do:
Theta*: Any-Angle Path Planning for Smoother Trajectories in Continuous Environments

Add polynomials together using recursion

I need to add two polynomials together using a recursive method.
This is for a past-due assignment (I imagine a similar thing will be on a test).
Main class:
public class Polynomial {
private Node poly;
public Polynomial(){
poly = new Node();
}
private Polynomial(Node node){
poly = node;
}
public void addTerm(int coef, int exp){
Term term = new Term(coef, exp);
Node node = new Node(term, null);
Node iterator = poly;
//if the list is empty just add the node
if (poly.next == null){
System.out.println("poly.next is null. adding node to first pos.");
poly.next = node;
return;
}
//if list isn't empty find the appropriate spot for it
while (iterator.next != null){
System.out.println("iterator.next != null...");
if (exp < iterator.next.data.exp){
System.out.println("\texp < iterator.next.data.exp");
node.next = iterator.next;
iterator.next = node;
return;
}
if (exp == iterator.next.data.exp){
System.out.println("\texp == iterator.next.data.exp");
iterator.next.data.coef += coef;
return;
}
iterator = iterator.next;
}
//if we get to this point then the list isn't empty
//and it doesn't fit inside the list, hence it must
//be added to the end of the list
System.out.println("list wasn't empty, didn't fit inside");
iterator.next = node;
return;
}
#Override
public String toString(){
Node iterator = poly;
String out = "";
if (poly.next == null){
return out;
}
while(iterator.next != null){
out += iterator.next.data.coef;
out += "*x^";
out += iterator.next.data.exp;
out += " + ";
iterator = iterator.next;
}
return out.substring(0, out.lastIndexOf('+'));
}
public Polynomial addPolynomial (Polynomial that){
Polynomial ret = new Polynomial();
Polynomial iterator = this;
return addPolynomial(that, ret, iterator);
}
public Polynomial addPolynomial(Polynomial that, Polynomial ret, Polynomial iterator){
if (iterator.poly.next == null){
return new Polynomial(that.poly);
}
if (that.poly.next == null){
return new Polynomial(iterator.poly);
}
if (iterator.poly.next.data.exp < that.poly.next.data.exp){
ret.addTerm(iterator.poly.next.data.coef, iterator.poly.next.data.exp);
iterator.poly = iterator.poly.next;
return iterator.addPolynomial(that);
}
if (iterator.poly.next.data.exp == that.poly.next.data.exp){
ret.addTerm(iterator.poly.next.data.coef + that.poly.next.data.coef, iterator.poly.next.data.exp);
iterator.poly = iterator.poly.next;
that.poly = that.poly.next;
return iterator.addPolynomial(that);
}
if (iterator.poly.next.data.exp > that.poly.next.data.exp){
ret.addTerm(that.poly.next.data.coef, that.poly.next.data.exp);
that.poly = that.poly.next;
return iterator.addPolynomial(that);
}
return ret;
}
/*
* Term
*/
private class Term implements Comparable{
int coef;
int exp;
public int getCoef() {
return coef;
}
public void setCoef(int coef) {
this.coef = coef;
}
public int getExp() {
return exp;
}
public void setExp(int exp) {
this.exp = exp;
}
public Term(int coef, int exp) {
this.coef = coef;
this.exp = exp;
}
public int compareTo(Object rhs){
Term that = (Term)rhs;
return this.exp - that.exp;
}
}//end Term
/*
* Node
*/
private class Node{
Term data;
Node next;
public Term getData() {
return data;
}
public void setData(Term data) {
this.data = data;
this.next = null;
}
public Node getNext() {
return next;
}
public void setNext(Node next) {
this.next = next;
}
public Node() {
this.data = null;
this.next = null;
}
public Node(Term data, Node next) {
this.data = data;
this.next = next;
}
}//end Node
}
Test Class:
public class Polynomials {
/**
* #param args the command line arguments
*/
public static void main(String[] args) {
Polynomial p1 = new Polynomial();
Polynomial p2 = new Polynomial();
Polynomial p0 = new Polynomial();
p1.addTerm(1, 2);
p1.addTerm(1, 4);
p1.addTerm(1, 6);
p2.addTerm(1, 1);
p2.addTerm(1, 3);
p2.addTerm(1, 5);
System.out.println("p1 = " + p1.toString());
System.out.println("p2 = " + p2.toString());
System.out.println("Adding p1 to p2...");
p0 = p1.addPolynomial(p2);
System.out.println(p0.toString());
}
}

Have you considered this way of doing it?
public class Polynomial {
public static void main(String[] args) {
Polynomial p1 = new Polynomial();
Polynomial p2 = new Polynomial();
Polynomial p0 = new Polynomial();
p1.addTerm(1, 2);
p1.addTerm(1, 4);
p1.addTerm(1, 6);
p2.addTerm(1, 1);
p2.addTerm(2, 3);
p2.addTerm(2, 2);
p2.addTerm(1, 5);
System.out.println("p1 = " + p1.toString());
System.out.println("p2 = " + p2.toString());
System.out.println("Adding p1 to p2...");
p0 = p1.addPolynomial(p2);
System.out.println(p0.toString());
for(int i = 0;i < 100;i++) {
p1 = new Polynomial();
p2 = new Polynomial();
for(int j = 0;j < 4;j++) {
p1.addTerm((int) (10 * Math.random()) - 5,
(int) (4 * Math.random()));
p2.addTerm((int) (10 * Math.random()) - 5,
(int) (4 * Math.random()));
}
p0 = p1.addPolynomial(p2);
System.out.println(p1 + "\n" + p2 + "\n" + p0 + "\n");
}
}
enum Comp {
LT, EQ, GT
}
static Comp cmp(int a, int b) {
return (a < b) ? Comp.LT : (a == b) ? Comp.EQ : Comp.GT;
}
private Term poly;
public Polynomial() {
poly = null;
}
public void addTerm(int coef, int exp) {
if (coef == 0) return;
Term term = new Term(coef, exp,null);
if (poly == null) {
poly = term;
} else {
poly = poly.add(term);
}
}
#Override
public String toString() {
if (poly == null) return "0";
StringBuilder buf = new StringBuilder();
poly.writeTo(buf);
return buf.toString();
}
public Polynomial addPolynomial(Polynomial that) {
Polynomial ret = new Polynomial();
if (poly != null) {
ret.poly = new Term(poly);
if (that.poly != null) {
ret.poly = ret.poly.add(new Term(that.poly));
}
} else if (that.poly != null) {
ret.poly = new Term(that.poly);
}
return ret;
}
private class Term {
final int coef;
final int exp;
final Term next;
Term(int coef, int exp, Term next) {
this.coef = coef;
this.exp = exp;
this.next = next;
}
Term(Term copy) {
this.coef = copy.coef;
this.exp = copy.exp;
if (copy.next == null) {
this.next = null;
} else {
this.next = new Term(copy.next);
}
}
Term add(Term other) {
if (other == null) return this;
switch (cmp(this.exp, other.exp)) {
case LT: {
Term n = other.add(this);
return n;
}
case GT: {
if (next == null) {
return new Term(coef,exp,other);
}
return new Term(coef,exp,next.add(other));
}
default: {
Term n = (next==null) ? other.next : next.add(other.next);
int nc = coef+other.coef;
return (nc!=0) ? new Term(nc,exp,n) : n;
}
}
}
public void writeTo(StringBuilder app) {
if (coef != 1 || exp == 0) app.append(coef);
if (exp == 1) {
app.append("x");
} else if (exp != 0) {
app.append("x^").append(exp);
}
if (next != null) {
app.append('+');
next.writeTo(app);
}
}
}
}

Well that's certainly a bit more complex than necessary.
So you want to add a Polynomial to an already existing one, i.e.:
4*x^2 + 4 to 5*x^4+x+5, which results in: 5*x^4+4*x^2+x+9
You could obviously simplify the problem quite a bit by just using an array, but it's also not that hard with a linked list, the code should look something like this:
You have two pointers one pointing to the current position in the polynomial we want to add values (term1) and another one in the one we want to add (term2), both initialized to the lowest entry in the linked list (which we assume is sorted according to the exponent).
while term1 != NULL && term2 != NULL do:
term1 > term2:
add term2 to list
term2 = term2.next()
term2 > term1:
term1 = term1.next()
term1 = term2:
add term2 to term1
term1 = term1.next()
term2 = term2.next()
while term1 == NULL && term2 != NULL:
add term2
term2 = term2.next()
You can easily adapt that to also create a new polynomial instead of adding one to the other.
Okay so a little bit more details on how to turn the iterative solution into a recursive one:
The first thing you always have to do when thinking about recursion is your exit condition.
In this case if you think about that's easy: We only do work as long as term2 is not NULL, as soon as term2 is NULL we're finished.
The next step is thinking about a good method signature and that's also quite easy this time, since we only need the two terms, so you can just use the two iterators:
void addTogether(Iterator term1, Iterator term2); (assuming you're using an interface that's similar to the ListIterator, since you need to insert at the position before term1 - there are several ways to implement that)
ANd now you've just got to get rid of the while loops and replace them with a recursive call (in which you distinguish the different cases, do the necessary action and call it again)

Unable to implement A Star in java

I've been trying all day to get this algorithm up and running, but I cant for the life of me. I've read many tutorials on the net, and source code in AS3, javascript, and C++; but I cannot adapt what I am seeing to my own code.
I have created an AStar class that has a nested class named Node. The map is a 2D array named MAP.
The biggest problem that I am having is pulling the F value in the pathfind function.
I have implemented the F = G + H, my problem is the actual AStar algorithm. Can someone please help, this is how far I've got as of yet:
import java.util.ArrayList;
public class AStar
{
int MAP[][];
Node startNode, endNode;
public AStar(int MAP[][], int startXNode, int startYNode,
int endXNode, int endYNode)
{
this.MAP = MAP;
startNode = new Node(startXNode, startYNode);
endNode = new Node(endXNode, endYNode);
}
public void pathfinder()
{
ArrayList openList = new ArrayList();
ArrayList closedList = new ArrayList();
}
public int F(Node startNode, Node endNode)
{
return (H(startNode, endNode) + G(startNode));
}
//H or Heuristic part of A* algorithm
public int H(Node startNode, Node endNode)
{
int WEIGHT = 10;
int distance = (Math.abs(startNode.getX() - endNode.getX()) + Math.abs(startNode.getY() - endNode.getY()));
return (distance * WEIGHT);
}
public int G(Node startNode)
{
if(MAP[startNode.getX() - 1][startNode.getY()] != 1)
{
return 10;
}
if(MAP[startNode.getX() + 1][startNode.getY()] != 1)
{
return 10;
}
if(MAP[startNode.getX()][startNode.getY() -1] != 1)
{
return 10;
}
if(MAP[startNode.getX()][startNode.getY() + 1] != 1)
{
return 0;
}
return 0;
}
public class Node
{
private int NodeX;
private int NodeY;
private int gScore;
private int hScore;
private int fScore;
public Node(int NodeX, int NodeY)
{
this.NodeX = NodeX;
this.NodeY = NodeY;
}
public int getX()
{
return NodeX;
}
public int getY()
{
return NodeY;
}
public int getG()
{
return gScore;
}
public void setG(int gScore)
{
this.gScore = gScore;
}
public int getH()
{
return hScore;
}
public void setH(int hScore)
{
this.hScore = hScore;
}
public int getF()
{
return fScore;
}
public void setF(int fScore)
{
this.fScore = fScore;
}
}
}
This is the furthest I can ever get with the pathfinder function:
public void pathfinder()
{
LinkedList<Node> openList = new LinkedList();
LinkedList<Node> closedList = new LinkedList();
Node currentNode;
openList.add(startNode);
while(openList.size() > 0)
{
currentNode = (Node) openList.get(0);
closedList.add(currentNode);
for(int i = 0; i < openList.size(); i++)
{
int cost = F(currentNode, endNode);
}
}
}

I recently threw this A* code together to solve a Project Euler problem. You'll have to fill in the details for a matrix of Node objects. Use it at your own risk, however I can say it solved the problem :)
public class Node {
List<Node> neighbors = new ArrayList<Node>();
Node parent;
int f;
int g;
int h;
int x;
int y;
int cost;
}
public List<Node> aStar(Node start, Node goal) {
Set<Node> open = new HashSet<Node>();
Set<Node> closed = new HashSet<Node>();
start.g = 0;
start.h = estimateDistance(start, goal);
start.f = start.h;
open.add(start);
while (true) {
Node current = null;
if (open.size() == 0) {
throw new RuntimeException("no route");
}
for (Node node : open) {
if (current == null || node.f < current.f) {
current = node;
}
}
if (current == goal) {
break;
}
open.remove(current);
closed.add(current);
for (Node neighbor : current.neighbors) {
if (neighbor == null) {
continue;
}
int nextG = current.g + neighbor.cost;
if (nextG < neighbor.g) {
open.remove(neighbor);
closed.remove(neighbor);
}
if (!open.contains(neighbor) && !closed.contains(neighbor)) {
neighbor.g = nextG;
neighbor.h = estimateDistance(neighbor, goal);
neighbor.f = neighbor.g + neighbor.h;
neighbor.parent = current;
open.add(neighbor);
}
}
}
List<Node> nodes = new ArrayList<Node>();
Node current = goal;
while (current.parent != null) {
nodes.add(current);
current = current.parent;
}
nodes.add(start);
return nodes;
}
public int estimateDistance(Node node1, Node node2) {
return Math.abs(node1.x - node2.x) + Math.abs(node1.y - node2.y);
}

I dont know if you are trying only to use simple types, or if you just didn't think about it, but you need to have a PriorityQueue to get your A* working.
A good way to think is that you put your startpoint into a priority queue with distance 0, and then start a loop that only stops when the prioriy queue is empty.
In the loop you take the min-node out, and check to see if it hasnt been open before, or if it has, if you have now found a shorter way to it.
If either these are true, you add the distance to the new node, add the edge/from-square to a map, and then add the distance + heuristic to the priority queue.
I have written this to work on a grid of booleans, and a constant conversion between 1D and 2D arrays, but I hope it is readable:
public void AStarRoute()
{
gridDist = new double[rows][cols];
System.out.println("Start of AStarRoute");
MinPriorityQueue pq = new MinPriorityQueue(rows * cols);
edgeTo = new HashMap<Integer, Integer>();
gridDist[x1Dto2D(start)][y1Dto2D(start)] = 0;
pq.insert(start, 0);
int from;
while (!pq.isEmpty()) {
from = pq.delMin();
int x = x1Dto2D(from);
int y = y1Dto2D(from);
for (int i = -1; i <= 1; i++) {
for (int j = -1; j <= 1; j++) {
int newX = x + i;
int newY = y + j;
if (newX >= 0 && newY >= 0 && newX < cols && newY < rows && !(i == 0 && j == 0)) {
if (grid[newX][newY]) {
//System.out.println("NewDist: " + gridDist[newX][newY] + " - OldDist+dist: " + (gridDist[x][y] + ((Math.abs(i) == Math.abs(j)) ? 1.4 : 1.0)) + ":" + (int)(gridDist[x][y] + ((Math.abs(i) == Math.abs(j)) ? 1.4 : 1.0)));
if (!edgeTo.containsKey(convert2Dto1D(newX, newY)) || gridDist[newX][newY] > (gridDist[x][y] + ((Math.abs(i) == Math.abs(j)) ? 14 : 10))) {
gridDist[newX][newY] = (int)(gridDist[x][y] + ((Math.abs(i) == Math.abs(j)) ? 14 : 10));
maxDistToEnd = (int)Math.max(maxDistToEnd, gridDist[newX][newY]);
edgeTo.put(convert2Dto1D(newX, newY), convert2Dto1D(x, y));
pq.insert(convert2Dto1D(newX, newY), gridDist[newX][newY] + (int)Math.sqrt(Math.pow((newX - x1Dto2D(end))*10, 2) + Math.pow((newY - y1Dto2D(end))*10, 2)));
if(convert2Dto1D(newX, newY) == end){
System.out.println("End found at (" + newX + ", " + newY + ")");
paintGridDist = true;
route = new ArrayList<Integer>();
int n = convert2Dto1D(newX, newY);
route.add(n);
do{
n = edgeTo.get(n);
route.add(n);
}while(start != n);
repaint();
return;
}
}
}
}
}
}
}
paintGridDist = true;
repaint();
}