// Connect according to MPQ data public Spot AddAndConnectSpot(Spot s) { s = AddSpot(s); List <Spot> close = FindAllSpots(s.location, MaxStepLength); if (!s.GetFlag(Spot.FLAG_MPQ_MAPPED)) { foreach (Spot cs in close) { if (cs.HasPathTo(this, s) && s.HasPathTo(this, cs) || cs.IsBlocked()) { } else if (!triangleWorld.IsStepBlocked(s.X, s.Y, s.Z, cs.X, cs.Y, cs.Z, toonHeight, toonSize, null)) { float mid_x = (s.X + cs.X) / 2; float mid_y = (s.Y + cs.Y) / 2; float mid_z = (s.Z + cs.Z) / 2; float stand_z; int flags; if (triangleWorld.FindStandableAt(mid_x, mid_y, mid_z - WantedStepLength * .75f, mid_z + WantedStepLength * .75f, out stand_z, out flags, toonHeight, toonSize)) { s.AddPathTo(cs); cs.AddPathTo(s); } } } } return(s); }
// Connect according to MPQ data public Spot AddAndConnectSpot(Spot s) { s = AddSpot(s); List<Spot> close = FindAllSpots(s.location, MaxStepLength); if (!s.GetFlag(Spot.FLAG_MPQ_MAPPED)) { foreach (Spot cs in close) { if (cs.HasPathTo(this, s) && s.HasPathTo(this, cs) || cs.IsBlocked()) { } else if (!triangleWorld.IsStepBlocked(s.X, s.Y, s.Z, cs.X, cs.Y, cs.Z, toonHeight, toonSize, null)) { float mid_x = (s.X + cs.X) / 2; float mid_y = (s.Y + cs.Y) / 2; float mid_z = (s.Z + cs.Z) / 2; float stand_z; int flags; if (triangleWorld.FindStandableAt(mid_x, mid_y, mid_z - WantedStepLength * .75f, mid_z + WantedStepLength * .75f, out stand_z, out flags, toonHeight, toonSize)) { s.AddPathTo(cs); cs.AddPathTo(s); } } } } return s; }
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; System.DateTime pre = System.DateTime.Now; System.DateTime lastSpam = pre; // lowest first queue PriorityQueue <Spot, float> 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) { System.TimeSpan span = System.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 = System.DateTime.Now; prevCount = count; } } count++; if (spot.Equals(dst) || spot.location.GetDistanceTo(realDst) <= minHowClose) { System.TimeSpan ts = System.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; } { System.TimeSpan ts = System.DateTime.Now.Subtract(pre); if (ts.Seconds > 15) { Log("too long search, aborting"); break; } } float src_score = spot.SearchScoreGet(currentSearchID); //PPather.WriteLine("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)) { float 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 //PPather.WriteLine("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)) { Spot 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)) { // PPather.WriteLine("/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!?!? //PPather.WriteLine("/cry"); } } } } } } } } } { System.TimeSpan ts = System.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); // :( }