public float GetDistanceTo(Location l) { float dx = x - l.X; float dy = y - l.Y; float dz = z - l.Z; return (float)Math.Sqrt(dx * dx + dy * dy + dz * dz); }
public MoveResult move() { if (GiveUpIfUnsafe) // fixme { //if (!pather.IsItSafeAt(Me.Target, Me.Location)) // return MoveResult.Unsafe; } if (PathTimeout.IsReady) { MoveAlong = null; } BoogieCore.Log(LogType.System, "[Mover] MoveAlong = {0}", MoveAlong); if (MoveAlong == null) { Location from = new Location(Me.Location); mover.Stop(); Path path = world.CreatePath(from, target, (float)4.5, PPather.radar); //fixme PathTimeout.Reset(); BoogieCore.Log(LogType.System, "[Mover (New)] PathCount = {0}", path.Count()); if (path == null || path.Count() == 0) { //Context.Log("EasyMover: Can not create path . giving up"); mover.MoveRandom(); Thread.Sleep(200); mover.Stop(); return MoveResult.CantFindPath; } else { //Context.Log("Save path to " + pathfile); //path.Save(pathfile); MoveAlong = new MoveAlonger(pather, path); } } if (MoveAlong != null) { Location from = new Location(Me.Location); BoogieCore.Log(LogType.System, "[Mover ] PathCount = {0}", MoveAlong.path.Count()); if (MoveAlong.path.Count() == 0 || from.GetDistanceTo(target) < (float)5.0) { BoogieCore.Log(LogType.System, "[Mover] Count = 0 or Dist < 5.0"); MoveAlong = null; mover.Stop(); return MoveResult.GotThere; } else if (!MoveAlong.MoveAlong()) { BoogieCore.Log(LogType.System, "[Mover] Stuck!!"); MoveAlong = null; // got stuck! if (GiveUpIfStuck) return MoveResult.Stuck; } } return MoveResult.Moving; }
public EasyMover(PPather pather, Location target, bool GiveUpIfStuck, bool GiveUpIfUnsafe) { this.target = target; this.Me = BoogieCore.Player; this.world = pather.world; mover = PPather.mover; this.GiveUpIfStuck = GiveUpIfStuck; this.GiveUpIfUnsafe = GiveUpIfUnsafe; this.pather = pather; }
public void MarkBlockedAt(Location loc) { Spot s = new Spot(loc); s = AddSpot(s); s.SetFlag(Spot.FLAG_BLOCKED, true); // Find all paths leading though this one List<Spot> sl = FindAllSpots(loc, 5.0f); foreach (Spot sp in sl) { List<Location> paths = sp.GetPaths(); foreach (Location to in paths) { if (LineCrosses(sp.location, to, loc)) { sp.RemovePathTo(to); } } } }
public Spot(Location l) { this.X = l.X; this.Y = l.Y; this.Z = l.Z; }
public void AddLast(Location l) { locations.Add(l); }
public MoveAlonger(PPather pather, Path path) { this.Me = BoogieCore.Player; this.path = path; this.world = pather.world; mover = PPather.mover; sd = new StuckDetecter(pather, 1, 2); prev = null; current = path.GetFirst(); next = path.GetSecond(); }
private bool LineCrosses(Location line0, Location line1, Location point) { float LineMag = line0.GetDistanceTo(line1); // Magnitude( LineEnd, LineStart ); float U = (((point.X - line0.X) * (line1.X - line0.X)) + ((point.Y - line0.Y) * (line1.Y - line0.Y)) + ((point.Z - line0.Z) * (line1.Z - line0.Z))) / (LineMag * LineMag); if (U < 0.0f || U > 1.0f) return false; float InterX = line0.X + U * (line1.X - line0.X); float InterY = line0.Y + U * (line1.Y - line0.Y); float InterZ = line0.Z + U * (line1.Z - line0.Z); float Distance = point.GetDistanceTo(new Location(InterX, InterY, InterZ)); if (Distance < 0.5f) return true; return false; }
public void BlacklistStep(Location from, Location to) { Spot froms = GetSpot(from); if (froms != null) froms.RemovePathTo(to); }
public Path CreatePath(Location origin, Location destination, float dist, float hight, float radius) { this.toonHeight = hight; this.toonSize = radius; return CreatePath(origin, destination, dist); }
public List<Spot> FindAllSpots(Location l, float max_d) { List<Spot> sl = new List<Spot>(); int d = 0; while ((float)d <= max_d + 0.1f) { for (int i = -d; i <= d; i++) { float x_up = l.X + (float)d; float x_dn = l.X - (float)d; float y_up = l.Y + (float)d; float y_dn = l.Y - (float)d; Spot s0 = GetSpot2D(x_up, l.Y + i); Spot s2 = GetSpot2D(x_dn, l.Y + i); Spot s1 = GetSpot2D(l.X + i, y_dn); Spot s3 = GetSpot2D(l.X + i, y_up); Spot[] sv = { s0, s1, s2, s3 }; foreach (Spot s in sv) { Spot ss = s; while (ss != null) { float di = ss.GetDistanceTo(l); if (di < max_d) { sl.Add(ss); } ss = ss.next; if (Cancel) goto loop_end; } } } d++; } loop_end: return sl; }
public Path CreatePath(Location fromLoc, Location toLoc, float howClose) { return CreatePath(fromLoc, toLoc, howClose, null); }
public Path CreatePath(Location fromLoc, Location toLoc, float howClose, ILocationHeuristics locationHeuristics) { System.DateTime t = System.DateTime.Now; Spot from = FindClosestSpot(fromLoc, MinStepLength); Spot to = FindClosestSpot(toLoc, MinStepLength); if (from == null) { from = AddAndConnectSpot(new Spot(fromLoc)); } if (to == null) { to = AddAndConnectSpot(new Spot(toLoc)); } Path rawPath = CreatePath(from, to, to.location, howClose, true, locationHeuristics); if (rawPath != null && paint != null) { Location prev = null; for (int i = 0; i < rawPath.Count; i++) { Location l = rawPath.Get(i); paint.AddBigMarker(l.X, l.Y, l.Z); if (prev != null) { paint.PaintPath(l.X, l.Y, l.Z + 3, prev.X, prev.Y, prev.Z + 3); } prev = l; } } Console.WriteLine("CreatePath took {0}", System.DateTime.Now.Subtract(t).Seconds); if (rawPath == null) { return null; } else { Location last = rawPath.GetLast(); if (last.GetDistanceTo(toLoc) > 1.0) rawPath.AddLast(toLoc); } return rawPath; }
public Path CreatePath(Spot from, Spot to, Location realDst, float minHowClose, bool AllowInvented, ILocationHeuristics locationHeuristics) { Spot newTo = search(from, to, realDst, minHowClose, AllowInvented, locationHeuristics); if (newTo != null) { if (newTo.GetDistanceTo(to) <= minHowClose) { List<Spot> path = FollowTraceBack(from, newTo); return new Path(path); } } return null; }
public float GetDistanceTo(Location l) { float dx = l.X - X; float dy = l.Y - Y; float dz = l.Z - Z; return (float)Math.Sqrt(dx * dx + dy * dy + dz * dz); }
public void AddPathTo(Location l) { AddPathTo(l.X, l.Y, l.Z); }
public void MarkStuckAt(Location loc, float heading) { // TODO another day... Location inf = loc.InFrontOf(heading, 1.0f); MarkBlockedAt(inf); // TODO }
// this can be slow... public Spot FindClosestSpot(Location l_d) { return FindClosestSpot(l_d, 30.0f, null); }
public Spot TryAddSpot(Spot wasAt, Location isAt) { Spot isAtSpot = FindClosestSpot(isAt, WantedStepLength); if (isAtSpot == null) { isAtSpot = GetSpot(isAt); if (isAtSpot == null) { Spot s = new Spot(isAt); s = AddSpot(s); isAtSpot = s; } if (isAtSpot.GetFlag(Spot.FLAG_BLOCKED)) { isAtSpot.SetFlag(Spot.FLAG_BLOCKED, false); Log("Cleared blocked flag"); } if (wasAt != null) { wasAt.AddPathTo(isAtSpot); isAtSpot.AddPathTo(wasAt); } List<Spot> sl = FindAllSpots(isAtSpot.location, MaxStepLength); int connected = 0; foreach (Spot other in sl) { if (other != isAtSpot) { other.AddPathTo(isAtSpot); isAtSpot.AddPathTo(other); connected++; // Log(" connect to " + other.location); } } Log("Learned a new spot at " + isAtSpot.location + " connected to " + connected + " other spots"); wasAt = isAtSpot; } else { if (wasAt != null && wasAt != isAtSpot) { // moved to an old spot, make sure they are connected wasAt.AddPathTo(isAtSpot); isAtSpot.AddPathTo(wasAt); } wasAt = isAtSpot; } return wasAt; }
public Spot FindClosestSpot(Location l_d, Set<Spot> Not) { return FindClosestSpot(l_d, 30.0f, Not); }
private Spot search(Spot src, Spot dst, Location realDst, float minHowClose, bool AllowInvented, ILocationHeuristics locationHeuristics) { searchID++; int count = 0; Cancel = false; 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) && !Cancel) // && 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 > 120) { Log("too long search, aborting"); break; } } float src_score = spot.SearchScoreGet(currentSearchID); //Console.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++; } } if (Cancel) break; } //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 //Console.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)) { // Console.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!?!? //Console.WriteLine("/cry"); } } } } } } if (Cancel) break; } } } { 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; // :( }
public Spot FindClosestSpot(Location l, float max_d) { return FindClosestSpot(l, max_d, null); }
public void AddFirst(Location l) { locations.Insert(0, l); }
// this can be slow... public Spot FindClosestSpot(Location l, float max_d, Set<Spot> Not) { Spot closest = null; float closest_d = 1E30f; int d = 0; while ((float)d <= max_d + 0.1f) { for (int i = -d; i <= d; i++) { float x_up = l.X + (float)d; float x_dn = l.X - (float)d; float y_up = l.Y + (float)d; float y_dn = l.Y - (float)d; Spot s0 = GetSpot2D(x_up, l.Y + i); Spot s2 = GetSpot2D(x_dn, l.Y + i); Spot s1 = GetSpot2D(l.X + i, y_dn); Spot s3 = GetSpot2D(l.X + i, y_up); Spot[] sv = { s0, s1, s2, s3 }; foreach (Spot s in sv) { Spot ss = s; while (ss != null) { float di = ss.GetDistanceTo(l); if (di < max_d && !ss.IsBlocked() && (di < closest_d)) { closest = ss; closest_d = di; } ss = ss.next; if (Cancel) goto loop_end; } } } if (closest_d < d) // can't get better { //Log("Closest2 spot to " + l + " is " + closest); return closest; } d++; } loop_end: //Log("Closest1 spot to " + l + " is " + closest); return closest; }
public void SetNewTarget(Location target) { MoveAlong = null; this.target = target; }
public void RemovePathTo(Location l) { RemovePathTo(l.X, l.Y, l.Z); }
public bool MoveAlong() { double max = 3.0; Coordinate loc = Me.Location; Location isAt = new Location(loc.X, loc.Y, loc.Z); /* while (isAt.GetDistanceTo(current) < max && next != null) { //Context.Log(current + " - " + next); path.RemoveFirst(); if (path.Count() == 0) { //Context.Log("ya"); //return true; // good in some way } else { prev = current; current = path.GetFirst(); next = path.GetSecond(); } } */ bool consume = false; do { bool blocked = false; consume = false; if (next != null) world.triangleWorld.IsStepBlocked(loc.X, loc.Y, loc.Z, next.X, next.Y, next.Z, PathGraph.toonHeight, PathGraph.toonSize, null); double d = isAt.GetDistanceTo(current); if ((d < max && !blocked) || d < 1.5) consume = true; if (consume) { //GContext.Main.Log("Consume spot " + current + " d " + d + " block " + blocked); path.RemoveFirst(); if (path.Count() == 0) { break; } else { prev = current; current = path.GetFirst(); next = path.GetSecond(); } } } while (consume); { //Context.Log("Move towards " + current); Coordinate gto = new Coordinate((float)current.X, (float)current.Y, (float)current.Z); Coordinate face; if (next != null) face = new Coordinate(next.X, next.Y, next.Z); else face = gto; if (!mover.moveTowardsFacing(gto, 0.5, face)) { //Context.Log("Can't move " + current); world.BlacklistStep(prev, current); //world.MarkStuckAt(loc, Me.Heading); mover.MoveRandom(); Thread.Sleep(500); mover.Stop(); return false; // hmm, mover does not want to move, must be up or down } { double h; double speed; h = mover.GetMoveHeading(out speed); float stand_z = 0.0f; int flags = 0; float x = isAt.X + (float)Math.Cos(h) * 1.0f; float y = isAt.Y + (float)Math.Sin(h) * 1.0f; float z = isAt.Z; bool aheadOk = world.triangleWorld.FindStandableAt(x, y, z - 4, z + 6, out stand_z, out flags, 0, 0); if (!aheadOk) { blockCount++; world.MarkStuckAt(isAt, Me.Location.O); if (prev != null) { Coordinate gprev = new Coordinate((float)prev.X, (float)prev.Y, (float)prev.Z); if (!mover.moveTowardsFacing(gprev, 0.5, face)) { mover.Stop(); return false; } } if (blockCount > 1) { world.BlacklistStep(prev, current); return false; } return true; } else blockCount = 0; } if (sd.checkStuck()) { //Context.Log("Stuck at " + isAt); world.MarkStuckAt(isAt, (float)Me.Heading); world.BlacklistStep(prev, current); mover.Stop(); return false; } } return true; }
public bool IsUnderwaterOrInAir(Location l) { int flags; float z; if (triangleWorld.FindStandableAt(l.X, l.Y, l.Z - 50.0f, l.Z + 5.0f, out z, out flags, toonHeight, toonSize)) { if ((flags & ChunkedTriangleCollection.TriangleFlagDeepWater) != 0) return true; else return false; } //return true; return false; }
public Spot GetSpot(Location l) { if (l == null) return null; return GetSpot(l.X, l.Y, l.Z); }
public void SetLocation(Location l) { X = l.X; Y = l.Y; Z = l.Z; if (chunk != null) chunk.modified = true; }