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;
}