Merge branch
This commit is contained in:
commit
1535e1de10
1 changed files with 117 additions and 0 deletions
117
src/resonantinduction/contractor/Pathfinding.java
Normal file
117
src/resonantinduction/contractor/Pathfinding.java
Normal file
|
@ -0,0 +1,117 @@
|
|||
/**
|
||||
*
|
||||
*/
|
||||
package resonantinduction.contractor;
|
||||
|
||||
import java.util.HashMap;
|
||||
import java.util.HashSet;
|
||||
import java.util.Set;
|
||||
|
||||
import net.minecraftforge.common.ForgeDirection;
|
||||
import resonantinduction.base.Vector3;
|
||||
|
||||
/**
|
||||
* Uses the well known A* Pathfinding algorithm.
|
||||
*
|
||||
* @author Calclavia
|
||||
*
|
||||
*/
|
||||
public class Pathfinding
|
||||
{
|
||||
public Set<Vector3> openSet, closedSet;
|
||||
|
||||
public HashMap<Vector3, Vector3> navMap;
|
||||
|
||||
public HashMap<Vector3, Double> gScore, fScore;
|
||||
|
||||
public Vector3 target;
|
||||
|
||||
public Set<Vector3> results;
|
||||
|
||||
public Pathfinding(Vector3 target)
|
||||
{
|
||||
this.target = target;
|
||||
}
|
||||
|
||||
public boolean find(Vector3 start)
|
||||
{
|
||||
this.openSet.add(start);
|
||||
this.gScore.put(start, (double) 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;
|
||||
}
|
||||
|
||||
// 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));
|
||||
|
||||
double tentativeG = this.gScore.get(currentNode);
|
||||
|
||||
if (this.closedSet.contains(neighbor))
|
||||
{
|
||||
if (tentativeG >= this.gScore.get(neighbor))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
if (!this.openSet.contains(neighbor) || tentativeG < this.gScore.get(neighbor))
|
||||
{
|
||||
this.navMap.put(neighbor, currentNode);
|
||||
this.gScore.put(neighbor, tentativeG);
|
||||
this.fScore.put(neighbor, this.gScore.get(neighbor) + this.getEstimate(neighbor, this.target));
|
||||
this.openSet.add(neighbor);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
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 start.distance(target2);
|
||||
}
|
||||
}
|
Loading…
Reference in a new issue