public static ArrayList ReconstructPath(IDictionary cameFrom, AutoWaypoint currentNode) {
ArrayList path;
if (cameFrom.Contains(currentNode)) {
path = ReconstructPath(cameFrom, (AutoWaypoint)cameFrom[currentNode]);
path.Add(currentNode);
} else {
path = new ArrayList(10);
path.Add(currentNode);
}
return path;
}
public static AutoWaypoint ClosestWaypoint(Vector3 goal) {
AutoWaypoint closestPnt=null;
float closestDist = float.MaxValue;
float dist;
AutoWaypoint[] allPnts=AllWayPoints();
for (int i=0; i<allPnts.Length; ++i) {
dist = (allPnts[i].transform.position-goal).sqrMagnitude;
if (dist < closestDist) {
closestDist = dist;
closestPnt = allPnts[i];
}
}
return closestPnt;
}
public static ArrayList AStar(GameObject start, GameObject goal) {
//UnityEngine.Debug.Log("Finding path from " + start.transform.position.ToString() + " to " + goal.ToString());
Vector3 startPos = start.transform.position;
AutoWaypoint startWaypoint = ClosestWaypoint(startPos);
ConnectAllWaypoints();
//AutoWaypoint[] all= AllWayPoints();
//float tm = Time.realtimeSinceStartup;
ArrayList closedSet = new ArrayList(20);
ArrayList openSet = new ArrayList(20);
if (startWaypoint==null) {
Debug.LogWarning("No waypoints!");
return new ArrayList();
}
openSet.Add(startWaypoint);
IDictionary cameFrom = new Hashtable(20, 0.75f);
IDictionary g_score = new Hashtable(20, 0.75f);
IDictionary h_score = new Hashtable(20, 0.75f);
IDictionary f_score = new Hashtable(20, 0.75f);
g_score[startWaypoint] = 0.0f;
h_score[startWaypoint] = heuristicCostEstimate(startPos, goal.transform.position);
while(openSet.Count !=0) {
AutoWaypoint candidate=(AutoWaypoint)openSet[0];
if (((candidate.transform.position - goal.transform.position).sqrMagnitude <= A_STAR_GOAL_TOLERANCE_SQR) && candidate.CanSee(goal)) {
// Pretty close & can see:
//AutoWaypointOptions.Instance.lastWaypointFind = "" + (Time.realtimeSinceStartup-tm);
return ReconstructPath(cameFrom, candidate);
}
// Transfer waypoint to closed set:
closedSet.Add(candidate);
openSet.RemoveAt(0);
// Check neighbors (connected waypoints):
AutoWaypoint neighborCandidate;
float tentativeGScore;
bool tentativeIsBetter;
for (int i=0; i<candidate.waypoints.Length; ++i) {
neighborCandidate = candidate.waypoints[i];
if (closedSet.Contains(neighborCandidate)) {
continue;
}
// Calc scores & compare:
if (candidate.waypointDistances==null) { UnityEngine.Debug.Log(candidate + " has no distances."); }
tentativeGScore = ((g_score[candidate]==null) ? 0.0f : (float)g_score[candidate]);
tentativeGScore += (float)(candidate.waypointDistances[neighborCandidate]);
if (!openSet.Contains(neighborCandidate)) {
openSet.Add(neighborCandidate);
tentativeIsBetter=true;
} else if (tentativeGScore < ((g_score[neighborCandidate]==null) ? 0.0f : (float)g_score[neighborCandidate])) {
tentativeIsBetter=true;
} else {
tentativeIsBetter=false;
}
if (tentativeIsBetter) {
cameFrom[neighborCandidate] = candidate;
g_score[neighborCandidate] = tentativeGScore;
h_score[neighborCandidate] = heuristicCostEstimate(neighborCandidate.transform.position, goal.transform.position);
f_score[neighborCandidate] = ((g_score[neighborCandidate]==null) ? 0.0f : (float)g_score[neighborCandidate])
+ ((h_score[neighborCandidate]==null) ? 0.0f : (float)h_score[neighborCandidate]);
}
}
}
//AutoWaypointOptions.Instance.lastWaypointFind = "" + (Time.realtimeSinceStartup-tm);
return new ArrayList();
}
private float dist2; // To avoid many allocations/deletes
public WaypointComparer(AutoWaypoint sourcePoint)
{
srcPnt = sourcePoint;
}