From ff78509019bfdb8af70d4fd3cdfc07d9eebe8bd2 Mon Sep 17 00:00:00 2001
From: Calclavia <admin@calclavia.com>
Date: Sat, 3 Aug 2013 23:53:13 -0400
Subject: [PATCH] Added basic findNode function

---
 .../contractor/Pathfinding.java               | 52 ++++++++++++++++++-
 1 file changed, 51 insertions(+), 1 deletion(-)

diff --git a/src/resonantinduction/contractor/Pathfinding.java b/src/resonantinduction/contractor/Pathfinding.java
index 1cc64e66..4fcf6d9f 100644
--- a/src/resonantinduction/contractor/Pathfinding.java
+++ b/src/resonantinduction/contractor/Pathfinding.java
@@ -3,11 +3,61 @@
  */
 package resonantinduction.contractor;
 
+import java.util.HashMap;
+import java.util.Set;
+
+import resonantinduction.base.Vector3;
+
 /**
+ * Uses the well known A* Pathfinding algorithm.
+ * 
  * @author Calclavia
- *
+ * 
  */
 public class Pathfinding
 {
+	public Set<Vector3> openSet;
 
+	public HashMap<Vector3, Vector3> navMap;
+
+	public HashMap<Vector3, Double> gScore, fScore;
+
+	public Vector3 target;
+
+	public Pathfinding(Vector3 target)
+	{
+		this.target = target;
+	}
+
+	public void findNodes(Vector3 start)
+	{
+		this.openSet.add(start);
+		this.gScore.put(start, 0);
+		this.fScore.put(start, this.gScore.get(start) + getEstimate(start, this.target));
+
+		while (!this.openSet.isEmpty())
+		{
+			Vector3 currentNode = null;
+			double lowestFScore = 0;
+
+			for (Vector3 node : this.openSet)
+			{
+				if (currentNode == null || this.fScore.get(node) < lowestFScore)
+				{
+					currentNode = node;
+					lowestFScore = this.fScore.get(node);
+				}
+			}
+
+			if (currentNode == null)
+			{
+				break;
+			}
+		}
+	}
+
+	private double getEstimate(Vector3 start, Vector3 target2)
+	{
+		return null;
+	}
 }