private void checkGrid(Vector3 origin, CelestialBody body, VehicleMovementType vehicleType, float maxSlopeAngle, float minMass, float gridSize = GridSizeDefault)
{
if (grid == null || VectorUtils.GeoDistance(this.origin, origin, body) > rebuildDistance || Mathf.Abs(gridSize - GridSize) > 100 ||
this.body != body || movementType != vehicleType || this.maxSlopeAngle != maxSlopeAngle * Mathf.Deg2Rad)
{
GridSize = gridSize;
GridDiagonal = gridSize * Mathf.Sqrt(2);
this.body = body;
this.maxSlopeAngle = maxSlopeAngle * Mathf.Deg2Rad;
rebuildDistance = Mathf.Clamp(Mathf.Asin(MaxDistortion) * (float)body.Radius, GridSize * 4, GridSize * 256);
movementType = vehicleType;
this.origin = origin;
grid = new Dictionary <Coords, Cell>();
cornerAlts = new Dictionary <Coords, float>();
}
includeDebris(minMass);
}
/// <summary>
/// Check if line is traversable. Due to implementation specifics, it is advised not to use this if the start point is not the position of the vessel.
/// </summary>
/// <param name="startGeo">start point in Lat,Long,Alt form</param>
/// <param name="endGeo">end point, in Lat,Long,Alt form</param>
public bool TraversableStraightLine(Vector3 startGeo, Vector3 endGeo, CelestialBody body, VehicleMovementType vehicleType, float maxSlopeAngle, float minObstacleMass)
{
checkGrid(startGeo, body, vehicleType, maxSlopeAngle, minObstacleMass);
return(TraversableStraightLine(startGeo, endGeo));
}
/// <summary>
/// Create a new traversability matrix.
/// </summary>
/// <param name="start">Origin point, in Lat,Long,Alt form</param>
/// <param name="end">Destination point, in Lat,Long,Alt form</param>
/// <param name="body">Body on which the grid is created</param>
/// <param name="vehicleType">Movement type of the vehicle (surface/land)</param>
/// <param name="maxSlopeAngle">The highest slope angle (in degrees) the vessel can traverse in a straight line</param>
/// <returns>List of geo coordinate vectors of waypoints to traverse in straight lines to reach the destination</returns>
public List <Vector3> Pathfind(Vector3 start, Vector3 end, CelestialBody body, VehicleMovementType vehicleType, float maxSlopeAngle, float minObstacleMass)
{
checkGrid(start, body, vehicleType, maxSlopeAngle, minObstacleMass,
Mathf.Clamp(VectorUtils.GeoDistance(start, end, body) / 20, GridSizeDefault, GridSizeDefault * 5));
Coords startCoords = getGridCoord(start);
Coords endCoords = getGridCoord(end);
float initialDistance = gridDistance(startCoords, endCoords);
SortedDictionary <CellValue, float> sortedCandidates = new SortedDictionary <CellValue, float>(new CellValueComparer())
{
[new CellValue(getCellAt(startCoords), initialDistance)] = 0
}; //(openSet and fScore), gScore
Dictionary <Cell, float> candidates = new Dictionary <Cell, float>
{
[getCellAt(startCoords)] = initialDistance
}; // secondary dictionary to sortedCandidates for faster lookup
Dictionary <Cell, float> nodes = new Dictionary <Cell, float> //gScore
{
[getCellAt(startCoords)] = 0
};
Dictionary <Cell, Cell> backtrace = new Dictionary <Cell, Cell>(); //cameFrom
HashSet <Cell> visited = new HashSet <Cell>();
Cell current = null;
float currentFScore = 0;
KeyValuePair <Cell, float> best = new KeyValuePair <Cell, float>(getCellAt(startCoords), initialDistance * GiveUpHeuristicMultiplier);
List <KeyValuePair <Coords, float> > adjacent = new List <KeyValuePair <Coords, float> >(8)
{
new KeyValuePair <Coords, float>(new Coords(0, 1), GridSize),
new KeyValuePair <Coords, float>(new Coords(1, 0), GridSize),
new KeyValuePair <Coords, float>(new Coords(0, -1), GridSize),
new KeyValuePair <Coords, float>(new Coords(-1, 0), GridSize),
new KeyValuePair <Coords, float>(new Coords(1, 1), GridDiagonal),
new KeyValuePair <Coords, float>(new Coords(1, -1), GridDiagonal),
new KeyValuePair <Coords, float>(new Coords(-1, -1), GridDiagonal),
new KeyValuePair <Coords, float>(new Coords(-1, 1), GridDiagonal),
};
while (candidates.Count > 0)
{
// take the best candidate - since now we use SortedDict, it's the first one
using (var e = sortedCandidates.GetEnumerator())
{
e.MoveNext();
current = e.Current.Key.Cell;
currentFScore = e.Current.Key.Value;
candidates.Remove(e.Current.Key.Cell);
sortedCandidates.Remove(e.Current.Key);
}
// stop if we found our destination
if (current.Coords == endCoords)
{
break;
}
if (currentFScore > best.Value)
{
current = best.Key;
break;
}
visited.Add(current);
float currentNodeScore = nodes[current];
using (var adj = adjacent.GetEnumerator())
while (adj.MoveNext())
{
Cell neighbour = getCellAt(current.Coords + adj.Current.Key);
if (!neighbour.Traversable || visited.Contains(neighbour))
{
continue;
}
if (candidates.TryGetValue(neighbour, out float value))
{
if (currentNodeScore + adj.Current.Value >= value)
{
continue;
}
else
{
sortedCandidates.Remove(new CellValue(neighbour, value)); //we'll reinsert with the adjusted value, so it's sorted properly
}
}
nodes[neighbour] = currentNodeScore + adj.Current.Value;
backtrace[neighbour] = current;
float remainingDistanceEstimate = gridDistance(neighbour.Coords, endCoords);
float fScoreEstimate = currentNodeScore + adj.Current.Value + remainingDistanceEstimate * RetraceReluctanceMultiplier;
sortedCandidates[new CellValue(neighbour, fScoreEstimate)] = currentNodeScore + adj.Current.Value;
candidates[neighbour] = currentNodeScore + adj.Current.Value;
if ((fScoreEstimate + remainingDistanceEstimate * (GiveUpHeuristicMultiplier - 1)) < best.Value)
{
best = new KeyValuePair <Cell, float>(neighbour, fScoreEstimate + remainingDistanceEstimate * (GiveUpHeuristicMultiplier - 1));
}
}
}
var path = new List <Cell>();
while (current.Coords != startCoords)
{
path.Add(current);
current = backtrace[current];
}
path.Reverse();
if (path.Count > 2)
{
var newPath = new List <Cell>()
{
path[0]
};
for (int i = 1; i < path.Count - 1; ++i)
{
if (path[i].Coords - path[i - 1].Coords != path[i + 1].Coords - path[1].Coords)
{
newPath.Add(path[i]);
}
}
newPath.Add(path[path.Count - 1]);
path = newPath;
}
var pathReduced = new List <Vector3>();
Coords waypoint = startCoords;
for (int i = 1; i < path.Count; ++i)
{
if (!straightPath(waypoint.X, waypoint.Y, path[i].X, path[i].Y))
{
pathReduced.Add(path[i - 1].GeoPos);
waypoint = path[i - 1].Coords;
}
}
// if not path found
if (path.Count == 0)
{
if (startCoords == endCoords)
{
pathReduced.Add(end);
}
else
{
pathReduced.Add(start);
}
}
else if (path[path.Count - 1].Coords == endCoords)
{
pathReduced.Add(end);
}
else
{
pathReduced.Add(path[path.Count - 1].GeoPos);
}
return(pathReduced);
}