More work on pathfinder

src/resonantinduction/contractor/Pathfinding.java

@@ -4,8 +4,10 @@
 package resonantinduction.contractor;
 
 import java.util.HashMap;
+import java.util.HashSet;
 import java.util.Set;
 
+import net.minecraftforge.common.ForgeDirection;
 import resonantinduction.base.Vector3;
 
 /**
@@ -16,7 +18,7 @@ import resonantinduction.base.Vector3;
  */
 public class Pathfinding
 {
-	public Set<Vector3> openSet;
+	public Set<Vector3> openSet, closedSet;
 
 	public HashMap<Vector3, Vector3> navMap;
 
@@ -24,15 +26,17 @@ public class Pathfinding
 
 	public Vector3 target;
 
+	public Set<Vector3> results;
+
 	public Pathfinding(Vector3 target)
 	{
 		this.target = target;
 	}
 
-	public void findNodes(Vector3 start)
+	public boolean find(Vector3 start)
 	{
 		this.openSet.add(start);
-		this.gScore.put(start, 0);
+		this.gScore.put(start, (double) 0);
 		this.fScore.put(start, this.gScore.get(start) + getEstimate(start, this.target));
 
 		while (!this.openSet.isEmpty())
@@ -53,11 +57,42 @@ public class Pathfinding
 			{
 				break;
 			}
+
+			// Break case here;
+
+			if (currentNode.equals(this.target))
+			{
+				this.results = this.reconstructPath(this.navMap, this.target);
+				return true;
+			}
+
+			this.openSet.remove(currentNode);
+			this.closedSet.add(currentNode);
+
+			for (int i = 0; i < 6; i++)
+			{
+				ForgeDirection direction = ForgeDirection.getOrientation(i);
+				Vector3 neighbor = currentNode.clone().translate(new Vector3(direction.offsetX, direction.offsetY, direction.offsetZ));
+				
+			}
 		}
 	}
 
+	private Set<Vector3> reconstructPath(HashMap<Vector3, Vector3> naviMap, Vector3 currentNode)
+	{
+		Set<Vector3> path = new HashSet<Vector3>();
+		path.add(currentNode);
+
+		if (naviMap.containsKey(currentNode))
+		{
+			path.addAll(this.reconstructPath(naviMap, currentNode));
+		}
+
+		return path;
+	}
+
 	private double getEstimate(Vector3 start, Vector3 target2)
 	{
-		return null;
+		return start.distance(target2);
 	}
 }