public void ScoreSpot_A_Star_With_Model_And_Gradient_Avoidance(Spot spotLinkedToCurrent, Spot destinationSpot, int currentSearchID, ILocationHeuristics locationHeuristics, PriorityQueue <Spot, float> prioritySpotQueue) { //score spot float G_Score = currentSearchSpot.traceBackDistance + currentSearchSpot.GetDistanceTo(spotLinkedToCurrent); // the movement cost to move from the starting point A to a given square on the grid, following the path generated to get there. float H_Score = spotLinkedToCurrent.GetDistanceTo2D(destinationSpot) * heuristicsFactor; // the estimated movement cost to move from that given square on the grid to the final destination, point B. This is often referred to as the heuristic, which can be a bit confusing. The reason why it is called that is because it is a guess. We really don�t know the actual distance until we find the path, because all sorts of things can be in the way (walls, water, etc.). You are given one way to calculate H in this tutorial, but there are many others that you can find in other articles on the web. float F_Score = G_Score + H_Score; if (spotLinkedToCurrent.IsFlagSet(Spot.FLAG_WATER)) { F_Score += 30; } int score = GetTriangleClosenessScore(spotLinkedToCurrent.location); score += GetTriangleGradiantScore(spotLinkedToCurrent.location, gradiantMax); F_Score += score * 2; if (!spotLinkedToCurrent.SearchScoreIsSet(currentSearchID) || F_Score < spotLinkedToCurrent.SearchScoreGet(currentSearchID)) { // shorter path to here found spotLinkedToCurrent.traceBack = currentSearchSpot; spotLinkedToCurrent.traceBackDistance = G_Score; spotLinkedToCurrent.SearchScoreSet(currentSearchID, F_Score); prioritySpotQueue.Enqueue(spotLinkedToCurrent, -F_Score); } }
public void ScoreSpot_Pather(Spot spotLinkedToCurrent, Spot destinationSpot, int currentSearchID, ILocationHeuristics locationHeuristics, PriorityQueue <Spot, float> prioritySpotQueue) { //score spots float currentSearchSpotScore = currentSearchSpot.SearchScoreGet(currentSearchID); float linkedSpotScore = 1E30f; float new_score = currentSearchSpotScore + currentSearchSpot.GetDistanceTo(spotLinkedToCurrent) + TurnCost(currentSearchSpot, spotLinkedToCurrent); if (locationHeuristics != null) { new_score += locationHeuristics.Score(currentSearchSpot.X, currentSearchSpot.Y, currentSearchSpot.Z); } if (spotLinkedToCurrent.IsFlagSet(Spot.FLAG_WATER)) { new_score += 30; } if (spotLinkedToCurrent.SearchScoreIsSet(currentSearchID)) { linkedSpotScore = spotLinkedToCurrent.SearchScoreGet(currentSearchID); } if (new_score < linkedSpotScore) { // shorter path to here found spotLinkedToCurrent.traceBack = currentSearchSpot; spotLinkedToCurrent.SearchScoreSet(currentSearchID, new_score); prioritySpotQueue.Enqueue(spotLinkedToCurrent, -(new_score + spotLinkedToCurrent.GetDistanceTo(destinationSpot) * heuristicsFactor)); } }
// return null if failed or the last spot in the path found private Spot search(Spot src, Spot dst, Location realDst, float minHowClose, bool AllowInvented, ILocationHeuristics locationHeuristics) { searchID++; int count = 0; int prevCount = 0; int currentSearchID = searchID; float heuristicsFactor = 1.3f; DateTime pre = DateTime.Now; DateTime lastSpam = pre; // lowest first queue var q = new PriorityQueue <Spot, float>(); // (new SpotSearchComparer(dst, score)); ; q.Enqueue(src, -src.GetDistanceTo(dst) * heuristicsFactor); Spot BestSpot = null; //Set<Spot> closed = new Set<Spot>(); //SpotData<float> score = new SpotData<float>(); src.SearchScoreSet(currentSearchID, 0.0f); src.traceBack = null; // A* -ish algorithm while (q.Count != 0) // && count < 100000) { float prio; Spot spot = q.Dequeue(out prio); // .Value; //q.Remove(spot); if (spot.SearchIsClosed(currentSearchID)) { continue; } spot.SearchClose(currentSearchID); if (count % 100 == 0) { TimeSpan span = DateTime.Now.Subtract(lastSpam); if (span.Seconds != 0 && BestSpot != null) { Thread.Sleep(50); // give glider a chance to stop us int t = span.Seconds * 1000 + span.Milliseconds; if (t == 0) { Log("searching.... " + (count + 1) + " d: " + BestSpot.location.GetDistanceTo(realDst)); } else { Log("searching.... " + (count + 1) + " d: " + BestSpot.location.GetDistanceTo(realDst) + " " + (count - prevCount) * 1000 / t + " steps/s"); } lastSpam = DateTime.Now; prevCount = count; } } count++; if (spot.Equals(dst) || spot.location.GetDistanceTo(realDst) <= minHowClose) { TimeSpan ts = DateTime.Now.Subtract(pre); int t = ts.Seconds * 1000 + ts.Milliseconds; /*if(t == 0) * Log(" search found the way there. " + count); * else * Log(" search found the way there. " + count + " " + (count * 1000) / t + " steps/s"); */ return(spot); // got there } if (BestSpot == null || spot.location.GetDistanceTo(realDst) < BestSpot.location.GetDistanceTo(realDst)) { BestSpot = spot; } { TimeSpan ts = DateTime.Now.Subtract(pre); if (ts.Seconds > 15) { Log("too long search, aborting"); break; } } float src_score = spot.SearchScoreGet(currentSearchID); //PathGraph.Log("inspect: " + c + " score " + s); int new_found = 0; List <Spot> ll = spot.GetPathsToSpots(this); foreach (Spot to in ll) { //Spot to = GetSpot(l); if (to != null && !to.IsBlocked() && !to.SearchIsClosed(currentSearchID)) { float old_score = 1E30f; float new_score = src_score + spot.GetDistanceTo(to) + TurnCost(spot, to); if (locationHeuristics != null) { new_score += locationHeuristics.Score(spot.X, spot.Y, spot.Z); } if (to.GetFlag(Spot.FLAG_WATER)) { new_score += 30; } if (to.SearchScoreIsSet(currentSearchID)) { old_score = to.SearchScoreGet(currentSearchID); } if (new_score < old_score) { // shorter path to here found to.traceBack = spot; //if (q.Contains(to)) // q.Remove(to); // very sloppy to not dequeue it to.SearchScoreSet(currentSearchID, new_score); q.Enqueue(to, -(new_score + to.GetDistanceTo(dst) * heuristicsFactor)); new_found++; } } } //hmm search the triangles :p if (!spot.GetFlag(Spot.FLAG_MPQ_MAPPED)) { var PI = (float)Math.PI; spot.SetFlag(Spot.FLAG_MPQ_MAPPED, true); for (float a = 0; a < PI * 2; a += PI / 8) { float nx = spot.X + (float)Math.Sin(a) * WantedStepLength; // *0.8f; float ny = spot.Y + (float)Math.Cos(a) * WantedStepLength; // *0.8f; Spot s = GetSpot(nx, ny, spot.Z); if (s == null) { s = FindClosestSpot(new Location(nx, ny, spot.Z), MinStepLength); // TODO: this is slow } if (s != null) { // hmm, they should already be connected } else { float new_z; int flags; // gogo find a new one //PathGraph.Log("gogo brave new world"); if (!triangleWorld.FindStandableAt(nx, ny, spot.Z - WantedStepLength * .75f, spot.Z + WantedStepLength * .75f, out new_z, out flags, toonHeight, toonSize)) { //Spot blocked = new Spot(nx, ny, spot.Z); //blocked.SetFlag(Spot.FLAG_BLOCKED, true); //AddSpot(blocked); } else { s = FindClosestSpot(new Location(nx, ny, new_z), MinStepLength); if (s == null) { if (!triangleWorld.IsStepBlocked(spot.X, spot.Y, spot.Z, nx, ny, new_z, toonHeight, toonSize, null)) { var n = new Spot(nx, ny, new_z); Spot to = AddAndConnectSpot(n); if ((flags & ChunkedTriangleCollection.TriangleFlagDeepWater) != 0) { to.SetFlag(Spot.FLAG_WATER, true); } if (((flags & ChunkedTriangleCollection.TriangleFlagModel) != 0) || ((flags & ChunkedTriangleCollection.TriangleFlagObject) != 0)) { to.SetFlag(Spot.FLAG_INDOORS, true); } if (to != n || to.SearchIsClosed(currentSearchID)) { // PathGraph.Log("/sigh"); } else { // There should be a path from source to this one now if (spot.HasPathTo(to.location)) { float old_score = 1E30f; float new_score = src_score + spot.GetDistanceTo(to) + TurnCost(spot, to); if (locationHeuristics != null) { new_score += locationHeuristics.Score(spot.X, spot.Y, spot.Z); } if (to.GetFlag(Spot.FLAG_WATER)) { new_score += 30; } if (to.SearchScoreIsSet(currentSearchID)) { old_score = to.SearchScoreGet(currentSearchID); } if (new_score < old_score) { // shorter path to here found to.traceBack = spot; //if (q.Contains(to)) // q.Remove(to); to.SearchScoreSet(currentSearchID, new_score); q.Enqueue(to, -(new_score + to.GetDistanceTo(dst) * heuristicsFactor)); new_found++; } } else { // woot! I added a new one and it is not connected!?!? //PathGraph.Log("/cry"); } } } } } } } } } { TimeSpan ts = DateTime.Now.Subtract(pre); int t = ts.Seconds * 1000 + ts.Milliseconds; if (t == 0) { t = 1; } Log(" search failed. " + (count * 1000) / t + " steps/s"); } return(BestSpot); // :( }