public Spot TryAddSpot(Spot wasAt, Location isAt)
{
//if (IsUnderwaterOrInAir(isAt)) { return wasAt; }
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.IsFlagSet(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 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 void CreateSpotsAroundSpot(Spot currentSearchSpot, bool mapped)
{
if (!mapped)
{
//mark as mapped
currentSearchSpot.SetFlag(Spot.FLAG_MPQ_MAPPED, true);
float PI = (float)Math.PI;
//loop through the spots in a circle around the current search spot
for (float radianAngle = 0; radianAngle < PI * 2; radianAngle += PI / 8)
{
//calculate the location of the spot at the angle
float nx = currentSearchSpot.X + (float)Math.Sin(radianAngle) * WantedStepLength; // *0.8f;
float ny = currentSearchSpot.Y + (float)Math.Cos(radianAngle) * WantedStepLength; // *0.8f;
PeekSpot = new Spot(nx, ny, currentSearchSpot.Z);
//find the spot at this location, stop if there is one already
if (GetSpot(nx, ny, currentSearchSpot.Z) != null)
{
continue;
} //found a spot so don't create a new one
//see if there is a close spot, stop if there is
if (FindClosestSpot(new Location(nx, ny, currentSearchSpot.Z), MinStepLength) != null)
{
continue;
} // TODO: this is slow
// check we can stand at this new location
float new_z;
int flags;
if (!triangleWorld.FindStandableAt(nx, ny, currentSearchSpot.Z - WantedStepLength * .75f, currentSearchSpot.Z + WantedStepLength * .75f, out new_z, out flags, toonHeight, toonSize))
{
continue;
}
//see if a spot already exists at this location
if (FindClosestSpot(new Location(nx, ny, new_z), MinStepLength) != null)
{
continue;
}
//if the step is blocked then stop
if (triangleWorld.IsStepBlocked(currentSearchSpot.X, currentSearchSpot.Y, currentSearchSpot.Z, nx, ny, new_z, toonHeight, toonSize, null))
{
continue;
}
//create a new spot and connect it
Spot newSpot = AddAndConnectSpot(new Spot(nx, ny, new_z));
//PeekSpot = newSpot;
//check flags return by triangleWorld.FindStandableA
if ((flags & ChunkedTriangleCollection.TriangleFlagDeepWater) != 0)
{
newSpot.SetFlag(Spot.FLAG_WATER, true);
}
if (((flags & ChunkedTriangleCollection.TriangleFlagModel) != 0) || ((flags & ChunkedTriangleCollection.TriangleFlagObject) != 0))
{
newSpot.SetFlag(Spot.FLAG_INDOORS, true);
}
if (triangleWorld.IsCloseToModel(newSpot.X, newSpot.Y, newSpot.Z, IsCloseToModelRange))
{
newSpot.SetFlag(Spot.FLAG_CLOSETOMODEL, true);
}
}
}
}
// 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); // :(
}
