public Path CreatePath(Location fromLoc, Location toLoc, float howClose, ILocationHeuristics locationHeuristics)
{
logger.WriteLine("Creating Path from " + fromLoc.ToString() + " tot " + toLoc.ToString());
var sw = new Stopwatch();
sw.Start();
fromLoc = GetBestLocations(fromLoc);
toLoc = GetBestLocations(toLoc);
Spot from = FindClosestSpot("fromLoc", fromLoc, MinStepLength);
Spot to = FindClosestSpot("toLoc", toLoc, MinStepLength);
if (from == null)
{
from = AddAndConnectSpot(new Spot(fromLoc));
}
if (to == null)
{
to = AddAndConnectSpot(new Spot(toLoc));
}
Path rawPath = CreatePath(from, to, howClose, 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, prev.X, prev.Y, prev.Z);
}
prev = l;
}
}
logger.Debug(string.Format("CreatePath took {0} seconds.", sw.ElapsedMilliseconds / 1000));
if (rawPath == null)
{
return(null);
}
else
{
Location last = rawPath.GetLast();
if (last.GetDistanceTo(toLoc) > 1.0)
{
rawPath.AddLast(toLoc);
}
}
LastPath = rawPath;
return(rawPath);
}
public Path CreatePath(Location fromLoc, Location toLoc,
float howClose,
ILocationHeuristics locationHeuristics)
{
Stopwatch t = Stopwatch.StartNew();
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;
}
}
t.Stop();
Log("CreatePath took " + t.Elapsed);
if (rawPath == null)
{
return(null);
}
else
{
Location last = rawPath.GetLast();
if (last.GetDistanceTo(toLoc) > 1.0)
{
rawPath.AddLast(toLoc);
}
}
return(rawPath);
}
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(Location fromLoc, Location toLoc,
float howClose,
ILocationHeuristics locationHeuristics)
{
GSpellTimer t = new GSpellTimer(0);
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;
}
}
PPather.WriteLine("CreatePath took " + -t.TicksLeft);
if (rawPath == null)
{
return(null);
}
else
{
Location last = rawPath.GetLast();
if (last.GetDistanceTo(toLoc) > 1.0)
{
rawPath.AddLast(toLoc);
}
}
return(rawPath);
}
private Path CreatePath(Spot from, Spot to, float minHowClose, ILocationHeuristics locationHeuristics)
{
Spot newTo = Search(from, to, minHowClose, locationHeuristics);
if (newTo != null)
{
if (newTo.GetDistanceTo(to) <= minHowClose)
{
List <Spot> path = FollowTraceBack(from, newTo);
LastPath = new Path(path);
return(LastPath);
}
}
return(null);
}
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);
}
private Path CreatePath(Spot from, Spot to, float minHowClose, ILocationHeuristics locationHeuristics)
{
Spot newTo = Search(from, to, minHowClose, locationHeuristics);
if (newTo != null)
{
if (newTo.GetDistanceTo(to) <= MaximumAllowedRangeFromTarget)
{
List <Spot> path = FollowTraceBack(from, newTo);
LastPath = new Path(path);
return(LastPath);
}
else
{
logger.WriteLine($"Closest spot is too far from target. {newTo.GetDistanceTo(to)}>{MaximumAllowedRangeFromTarget}");
return(null);
}
}
return(null);
}
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));
}
}
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);
}
}
private void ScoreSpot(Spot spotLinkedToCurrent, Spot destinationSpot, int currentSearchID, ILocationHeuristics locationHeuristics, PriorityQueue <Spot, float> prioritySpotQueue)
{
switch (searchScoreSpot)
{
case eSearchScoreSpot.A_Star:
ScoreSpot_A_Star(spotLinkedToCurrent, destinationSpot, currentSearchID, locationHeuristics, prioritySpotQueue);
break;
case eSearchScoreSpot.A_Star_With_Model_Avoidance:
ScoreSpot_A_Star_With_Model_And_Gradient_Avoidance(spotLinkedToCurrent, destinationSpot, currentSearchID, locationHeuristics, prioritySpotQueue);
break;
case eSearchScoreSpot.OriginalPather:
default:
ScoreSpot_Pather(spotLinkedToCurrent, destinationSpot, currentSearchID, locationHeuristics, prioritySpotQueue);
break;
}
}
private Spot Search(Spot fromSpot, Spot destinationSpot, float minHowClose, ILocationHeuristics locationHeuristics)
{
var searchDuration = new Stopwatch();
searchDuration.Start();
var timeSinceProgress = new Stopwatch();
timeSinceProgress.Start();
var closest = 99999f;
ClosestSpot = null;
currentSearchStartSpot = fromSpot;
searchID++;
int currentSearchID = searchID;
//searchProgress = new SearchProgress(fromSpot, destinationSpot, searchID);
// lowest first queue
PriorityQueue <Spot, float> prioritySpotQueue = new PriorityQueue <Spot, float>(); // (new SpotSearchComparer(dst, score)); ;
prioritySpotQueue.Enqueue(fromSpot, -fromSpot.GetDistanceTo(destinationSpot) * heuristicsFactor);
fromSpot.SearchScoreSet(currentSearchID, 0.0f);
fromSpot.traceBack = null;
fromSpot.traceBackDistance = 0;
// A* -ish algorithm
while (prioritySpotQueue.Count != 0 && SearchEnabled)
{
if (sleepMSBetweenSpots != 0)
{
Thread.Sleep(sleepMSBetweenSpots);
} // slow down the pathing
float prio;
currentSearchSpot = prioritySpotQueue.Dequeue(out prio); // .Value;
// force the world to be loaded
TriangleCollection tc = triangleWorld.GetChunkAt(currentSearchSpot.X, currentSearchSpot.Y);
if (currentSearchSpot.SearchIsClosed(currentSearchID))
{
continue;
}
currentSearchSpot.SearchClose(currentSearchID);
//update status
//if (!searchProgress.CheckProgress(currentSearchSpot)) { break; }
// are we there?
var distance = currentSearchSpot.location.GetDistanceTo(destinationSpot.location);
if (distance <= minHowClose)
{
return(currentSearchSpot); // got there
}
if (distance < closest)
{
// spamming as hell
//logger.WriteLine($"Closet spot is {distance} from the target");
closest = distance;
ClosestSpot = currentSearchSpot;
PeekSpot = ClosestSpot;
timeSinceProgress.Reset();
timeSinceProgress.Start();
}
if (timeSinceProgress.Elapsed.TotalSeconds > ProgressTimeoutSeconds || searchDuration.Elapsed.TotalSeconds > TimeoutSeconds)
{
logger.WriteLine("search failed, 10 seconds since last progress, returning the closest spot.");
return(ClosestSpot);
}
//Find spots to link to
CreateSpotsAroundSpot(currentSearchSpot);
//score each spot around the current search spot and add them to the queue
foreach (Spot spotLinkedToCurrent in currentSearchSpot.GetPathsToSpots(this))
{
if (spotLinkedToCurrent != null && !spotLinkedToCurrent.IsBlocked() && !spotLinkedToCurrent.SearchIsClosed(currentSearchID))
{
ScoreSpot(spotLinkedToCurrent, destinationSpot, currentSearchID, locationHeuristics, prioritySpotQueue);
}
}
}
//we ran out of spots to search
//searchProgress.LogStatus(" search failed. ");
if (ClosestSpot != null && closest < MaximumAllowedRangeFromTarget)
{
logger.WriteLine("search failed, returning the closest spot.");
return(ClosestSpot);
}
return(null);
}
// 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); // :(
}
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 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;
}
private Spot Search(Spot fromSpot, Spot destinationSpot, float minHowClose, ILocationHeuristics locationHeuristics)
{
currentSearchStartSpot = fromSpot;
searchID++;
int currentSearchID = searchID;
//searchProgress = new SearchProgress(fromSpot, destinationSpot, searchID);
// lowest first queue
PriorityQueue <Spot, float> prioritySpotQueue = new PriorityQueue <Spot, float>(); // (new SpotSearchComparer(dst, score)); ;
prioritySpotQueue.Enqueue(fromSpot, -fromSpot.GetDistanceTo(destinationSpot) * heuristicsFactor);
fromSpot.SearchScoreSet(currentSearchID, 0.0f);
fromSpot.traceBack = null;
fromSpot.traceBackDistance = 0;
// A* -ish algorithm
while (prioritySpotQueue.Count != 0 && SearchEnabled)
{
if (sleepMSBetweenSpots != 0)
{
Thread.Sleep(sleepMSBetweenSpots);
} // slow down the pathing
float prio;
currentSearchSpot = prioritySpotQueue.Dequeue(out prio); // .Value;
// force the world to be loaded
TriangleCollection tc = triangleWorld.GetChunkAt(currentSearchSpot.X, currentSearchSpot.Y);
if (currentSearchSpot.SearchIsClosed(currentSearchID))
{
continue;
}
currentSearchSpot.SearchClose(currentSearchID);
//update status
//if (!searchProgress.CheckProgress(currentSearchSpot)) { break; }
// are we there?
if (currentSearchSpot.location.GetDistanceTo(destinationSpot.location) <= minHowClose)
{
return(currentSearchSpot); // got there
}
//Find spots to link to
CreateSpotsAroundSpot(currentSearchSpot);
//score each spot around the current search spot and add them to the queue
foreach (Spot spotLinkedToCurrent in currentSearchSpot.GetPathsToSpots(this))
{
if (spotLinkedToCurrent != null && !spotLinkedToCurrent.IsBlocked() && !spotLinkedToCurrent.SearchIsClosed(currentSearchID))
{
ScoreSpot(spotLinkedToCurrent, destinationSpot, currentSearchID, locationHeuristics, prioritySpotQueue);
}
}
}
//we ran out of spots to search
//searchProgress.LogStatus(" search failed. ");
return(null); // :(
}
