113 lines
3.9 KiB
Java
113 lines
3.9 KiB
Java
package universalelectricity.core.path;
|
|
|
|
import java.util.HashMap;
|
|
import java.util.HashSet;
|
|
import java.util.Iterator;
|
|
import java.util.Set;
|
|
import net.minecraftforge.common.util.ForgeDirection;
|
|
import universalelectricity.core.path.IPathCallBack;
|
|
import universalelectricity.core.path.Pathfinder;
|
|
import universalelectricity.core.vector.Vector3;
|
|
|
|
public class PathfinderAStar extends Pathfinder {
|
|
|
|
public IPathCallBack callBackCheck;
|
|
public Set openSet;
|
|
public HashMap navigationMap;
|
|
public HashMap gScore;
|
|
public HashMap fScore;
|
|
public Vector3 goal;
|
|
|
|
|
|
public PathfinderAStar(IPathCallBack callBack, Vector3 goal) {
|
|
super(callBack);
|
|
this.goal = goal;
|
|
}
|
|
|
|
public boolean findNodes(Vector3 start) {
|
|
this.openSet.add(start);
|
|
this.gScore.put(start, Double.valueOf(0.0D));
|
|
this.fScore.put(start, Double.valueOf(((Double)this.gScore.get(start)).doubleValue() + this.getHeuristicEstimatedCost(start, this.goal)));
|
|
|
|
while(!this.openSet.isEmpty()) {
|
|
Vector3 currentNode = null;
|
|
double lowestFScore = 0.0D;
|
|
Iterator i$ = this.openSet.iterator();
|
|
|
|
Vector3 neighbor;
|
|
while(i$.hasNext()) {
|
|
neighbor = (Vector3)i$.next();
|
|
if(currentNode == null || ((Double)this.fScore.get(neighbor)).doubleValue() < lowestFScore) {
|
|
currentNode = neighbor;
|
|
lowestFScore = ((Double)this.fScore.get(neighbor)).doubleValue();
|
|
}
|
|
}
|
|
|
|
if(currentNode == null) {
|
|
break;
|
|
}
|
|
|
|
if(this.callBackCheck.onSearch(this, currentNode)) {
|
|
return false;
|
|
}
|
|
|
|
if(currentNode.equals(this.goal)) {
|
|
super.results = this.reconstructPath(this.navigationMap, this.goal);
|
|
return true;
|
|
}
|
|
|
|
this.openSet.remove(currentNode);
|
|
super.closedSet.add(currentNode);
|
|
i$ = this.getNeighborNodes(currentNode).iterator();
|
|
|
|
while(i$.hasNext()) {
|
|
neighbor = (Vector3)i$.next();
|
|
double tentativeGScore = ((Double)this.gScore.get(currentNode)).doubleValue() + currentNode.distanceTo(neighbor);
|
|
if((!super.closedSet.contains(neighbor) || tentativeGScore < ((Double)this.gScore.get(neighbor)).doubleValue()) && (!this.openSet.contains(neighbor) || tentativeGScore < ((Double)this.gScore.get(neighbor)).doubleValue())) {
|
|
this.navigationMap.put(neighbor, currentNode);
|
|
this.gScore.put(neighbor, Double.valueOf(tentativeGScore));
|
|
this.fScore.put(neighbor, Double.valueOf(((Double)this.gScore.get(neighbor)).doubleValue() + this.getHeuristicEstimatedCost(neighbor, this.goal)));
|
|
this.openSet.add(neighbor);
|
|
}
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
public Pathfinder reset() {
|
|
this.openSet = new HashSet();
|
|
this.navigationMap = new HashMap();
|
|
return super.reset();
|
|
}
|
|
|
|
public Set reconstructPath(HashMap nagivationMap, Vector3 current_node) {
|
|
HashSet path = new HashSet();
|
|
path.add(current_node);
|
|
if(nagivationMap.containsKey(current_node)) {
|
|
path.addAll(this.reconstructPath(nagivationMap, (Vector3)nagivationMap.get(current_node)));
|
|
return path;
|
|
} else {
|
|
return path;
|
|
}
|
|
}
|
|
|
|
public double getHeuristicEstimatedCost(Vector3 start, Vector3 goal) {
|
|
return start.distanceTo(goal);
|
|
}
|
|
|
|
public Set getNeighborNodes(Vector3 vector) {
|
|
if(this.callBackCheck != null) {
|
|
return this.callBackCheck.getConnectedNodes(this, vector);
|
|
} else {
|
|
HashSet neighbors = new HashSet();
|
|
|
|
for(int i = 0; i < 6; ++i) {
|
|
neighbors.add(vector.clone().modifyPositionFromSide(ForgeDirection.getOrientation(i)));
|
|
}
|
|
|
|
return neighbors;
|
|
}
|
|
}
|
|
}
|