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);
}
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); // :(
}