Hi all, I seem to be either losing my mind or misimplementing the A* algorithm:
Below is my code, it seems that no matter what values I enter it will always return 360. Am I missing some critical piece of information here? Also before anyone asks yes this is related to a machine learning assignment I received.
public class A_Star {
private ArrayList<SearchNode> closedNodes = new ArrayList<SearchNode>();
private ArrayList<SearchNode> openNodes = new ArrayList<SearchNode>();
private ArrayList<SearchNode> siblingNodes = new ArrayList<SearchNode>();
private ArrayList<SearchNode> obstacleNodes;
final SearchNode START_NODE = new SearchNode(0,115,655);
final SearchNode END_NODE = new SearchNode(0,380,560);
final int HEURISTIC = 1 * Math.abs((END_NODE.getxCoordinate() - START_NODE.getxCoordinate()) + (START_NODE.getyCoordinate() - END_NODE.getyCoordinate())) ;
private int cost = 0;
//Start: (115, 655) End: (380, 560)
public int starSearch(SearchNode currentNode, Set<SearchNode> obstacleNodes) throws Exception {
//h(n) = D * (abs(n.x-goal.x) + abs(n.y-goal.y))
boolean isMaxY = false;
boolean isMaxX = false;
int currentY = currentNode.getyCoordinate();
int currentX = currentNode.getxCoordinate();
System.out.println(currentNode.getxCoordinate() + " " + currentNode.getyCoordinate() + " Current Coordinates");
int currentGScore = Math.abs(currentNode.getxCoordinate() - END_NODE.getxCoordinate()) + (currentNode.getyCoordinate() - END_NODE.getyCoordinate());
currentNode.setgScore(currentGScore);
System.out.println("Current node G score at entry: " + currentNode.getgScore());
if(!closedNodes.contains(currentNode)){
closedNodes.add(currentNode);
}
if(currentNode.getyCoordinate() == END_NODE.getyCoordinate()){
isMaxY=true;
}
if(currentNode.getxCoordinate() == END_NODE.getxCoordinate()) {
isMaxX =true;
}
SearchNode bottomCenter = new SearchNode(0,currentNode.getxCoordinate(), currentNode.getyCoordinate() -1);
SearchNode middleRight = new SearchNode(0,currentNode.getxCoordinate() +1, currentNode.getyCoordinate() );
SearchNode middleLeft = new SearchNode(0,currentNode.getxCoordinate() -1, currentNode.getyCoordinate());
SearchNode topCenter = new SearchNode(0,currentNode.getxCoordinate(), currentNode.getyCoordinate()+1);
int middleRightGScore = Math.abs(middleRight.getxCoordinate() - END_NODE.getxCoordinate()) + (middleRight.getyCoordinate() - END_NODE.getyCoordinate());
int bottomCenterGScore = Math.abs(bottomCenter.getxCoordinate() - END_NODE.getxCoordinate()) + (bottomCenter.getyCoordinate() - END_NODE.getyCoordinate());
int middleLeftGScore = Math.abs(middleLeft.getxCoordinate() - END_NODE.getxCoordinate()) + (middleLeft.getyCoordinate() - END_NODE.getyCoordinate());
int topCenterGScore = Math.abs(topCenter.getxCoordinate() - END_NODE.getxCoordinate()) + (topCenter.getyCoordinate() - END_NODE.getyCoordinate());
middleRight.setgScore(middleRightGScore);
middleLeft.setgScore(middleLeftGScore);
bottomCenter.setgScore(bottomCenterGScore);
topCenter.setgScore(topCenterGScore);
for(SearchNode obstacleNode:obstacleNodes){
int obstacleX = obstacleNode.getxCoordinate();
int obstacleY = obstacleNode.getyCoordinate();
if((middleRight != null) && (middleRight.getxCoordinate() == obstacleX)){
if(middleRight.getyCoordinate() == obstacleY){
// throw new Exception();
System.out.println("REMOVING AND NULLING: " + middleRight.toString());
siblingNodes.remove(middleRight);
middleRight = null;
}
}
if((middleLeft!=null)&&(middleLeft.getxCoordinate() == obstacleX)){
if(middleLeft.getyCoordinate() == obstacleY){
System.out.println("REMOVING AND NULLING: " + middleLeft.toString());
siblingNodes.remove(middleLeft);
middleLeft=null;
}
} if((bottomCenter!=null) &&(bottomCenter.getxCoordinate() == obstacleX)){
if(bottomCenter.getyCoordinate() == obstacleY){
System.out.println("REMOVING AND NULLING: " + bottomCenter.toString());
siblingNodes.remove(bottomCenter);
bottomCenter = null;
}
} if((topCenter!=null) && (topCenter.getxCoordinate() == obstacleX)){
if(topCenter.getyCoordinate() == obstacleY){
System.out.println("REMOVING AND NULLING: " + topCenter.toString());
siblingNodes.remove(topCenter);
topCenter=null;
}
}
if((middleRight != null) && (middleRight.getxCoordinate() != obstacleX)){
if(middleRight.getyCoordinate() != obstacleY){
System.out.println("ADDING THE FOLLOWING: " + middleRight.toString());
siblingNodes.add(middleRight);
}
}
if((middleLeft != null) && (middleLeft.getxCoordinate() != obstacleX)){
if(middleLeft.getyCoordinate() != obstacleY){
siblingNodes.add(middleLeft);
}
} if((bottomCenter != null) && (bottomCenter.getxCoordinate() != obstacleX)){
if(bottomCenter.getyCoordinate() != obstacleY){
siblingNodes.add(bottomCenter);
}
} if((topCenter !=null) && (topCenter.getxCoordinate() != obstacleX)){
if(topCenter.getyCoordinate() != obstacleY){
siblingNodes.add(topCenter);
}
}
}
int highestScore = currentNode.getgScore();
for(SearchNode node: siblingNodes){
if(node == null){
continue;
}
if(node.getxCoordinate() == END_NODE.getxCoordinate() && node.getyCoordinate() == END_NODE.getyCoordinate()){
System.out.println("Returning cost: " + ++cost + " of: " + node.toString());
return cost;
}
// System.out.println("Current node size: " + siblingNodes.size());
if(node.getgScore() < highestScore){
highestScore = node.getgScore();
currentNode=node;
cost++;
System.out.println("Changed highest score: " + highestScore);
System.out.println("Removing node: " + node.toString());
siblingNodes.remove(node);
System.out.println("Incrementing cost the Current Cost: " + cost);
starSearch(currentNode,obstacleNodes);
break;
}
if(isMaxY && isMaxX){
return cost;
}
}
return cost;
//Always move diagonal right downwards
}
public static void main(String[] args) throws Exception{
System.out.println("Hello");
A_Star star = new A_Star();
HashSet<SearchNode> obstacles = new HashSet<SearchNode>();
obstacles.add(new SearchNode(0,311,530));
obstacles.add(new SearchNode(0,311,559));
obstacles.add(new SearchNode(0,339,578));
obstacles.add(new SearchNode(0,361,560));
obstacles.add(new SearchNode(0,361,528));
obstacles.add(new SearchNode(0,116, 655));
int cost = star.starSearch(star.START_NODE, obstacles);
System.out.println(cost);
//((311, 530), (311, 559), (339, 578), (361, 560), (361, 528), (336, 516))
}
}
and
public class SearchNode {
private int xCoordinate;
private int yCoordinate;
private int cost;
public int getfScore() {
return fScore;
}
public void setfScore(int fScore) {
this.fScore = fScore;
}
private int fScore;
public int getgScore() {
return gScore;
}
public void setgScore(int gScore) {
this.gScore = gScore;
}
private int gScore;
public SearchNode(int cost, int xCoordinate,int yCoordinate){
this.cost=cost;
this.xCoordinate =xCoordinate;
this.yCoordinate = yCoordinate;
}
public int getCost() { return cost; }
public void setCost(int cost) {
this.cost = cost;
}
public int getxCoordinate() {
return xCoordinate;
}
public void setxCoordinate(int xCoordinate) {
this.xCoordinate = xCoordinate;
}
public int getyCoordinate() {
return yCoordinate;
}
public void setyCoordinate(int yCoordinate) {
this.yCoordinate = yCoordinate;
}
public String toString(){
return "Y Coordinate: " + this.yCoordinate + " X Coordinate: " + this.xCoordinate + " G Score: " + this.gScore;
}
}