private bool ConsiderRoute(
RoadCellLong pointA,
RoadCellLong pointB)
{
try
{
if (pointA.Previous == pointB)
{
return(false); //We've backtracked completely
}
if ((int)pointB.DistanceFromOrigin == 0)
{
pointB.DistanceFromOrigin = GetDirectLineDistance(_origin, pointB);
}
if (pointB != _destination && pointB.DistanceFromOrigin <= pointA.DistanceFromOrigin)
{
return(false); //We're circling back
}
//_logger.Info($"At [{pointB.X},{pointB.Z}] {pointB.DistanceFromOrigin} from Origin");
var costFromOrigin = pointA.CostFromOrigin + _getPathCost(pointA, pointB);
if (costFromOrigin > _destination.CostFromOrigin)
{
return(false);
}
// PointB has already been hit on different route, only redirect when the
// cost is lower.
if (pointB.Previous != null && pointB.CostFromOrigin <= costFromOrigin)
{
return(false);
}
// PointA already has a route assigned, only update it when the cost is lower
if (pointA.Next != null && pointA.Next.CostFromOrigin <= costFromOrigin)
{
return(false);
}
pointB.Previous = pointA;
pointA.Next = pointB;
pointB.CostFromOrigin = costFromOrigin;
if (pointB == _destination)
{
_maximumCost = costFromOrigin;
//_logger.Warn($"Found a faster route to destination [{pointA.X},{pointA.Z}] Cost: {_maximumCost}");
return(false);
}
return(pointA != pointB);
}
catch (Exception exp)
{
_logger.Error(exp);
throw;
}
}
private ulong GetUnitRouteCost(RoadCellLong pointA, RoadCellLong pointB)
{
var heightDifference = Math.Abs((long)pointA.Avg - (long)pointB.Avg);
if (heightDifference > 512)
{
return((ulong)heightDifference << 4);
}
return((ulong)heightDifference);
}
public async Task <RoadCellLong> BuildPath(RoadCellLong from, RoadCellLong to)
{
_origin = from;
_destination = to;
_getPathCost = (cell, roadCell) => WalkDirectEstimate(cell, roadCell, GetUnitRouteCost);
_destination.CostFromOrigin = long.MaxValue;
_maximumCost = _getPathCost(_origin, _destination);
_getNeighbours = Get4RadiusNeighbours;
await PathFindingUsingLinkedCells(from);
return(to);
}
private Vector2 <float> GetWalkerFactors(RoadCellLong origin, RoadCellLong destination)
{
var xDistance = destination.X - origin.X;
var zDistance = destination.Z - origin.Z;
var factors = new Vector2 <float>(Math.Sign(xDistance), Math.Sign(zDistance));
if (Math.Abs(xDistance) > Math.Abs(zDistance))
{
factors.Z = (float)zDistance / (float)Math.Abs(xDistance);
}
if (Math.Abs(zDistance) > Math.Abs(xDistance))
{
factors.X = (float)xDistance / (float)Math.Abs(zDistance);
}
return(factors);
}
private IEnumerable <RoadCellLong> GetRadiusNeighbours(RoadCellLong origin, List <Vector2 <int> > offSets, double backTrackDistanceCheck)
{
try
{
var neighbours = offSets
.Select(vector2 => new Vector2 <int>(vector2.X + origin.X, vector2.Z + origin.Z));
return(neighbours
.Where(vector2 => vector2.X > 0 && vector2.X < _size &&
vector2.Z > 0 && vector2.Z < _size)
.Select(vector2 => _map[vector2.X, vector2.Z]));
}
catch (Exception exp)
{
_logger.Error(exp);
throw;
}
}
public RoadCellLong GetCellMetrics(int x, int z)
{
var mapX = x * _cellSize;
var mapZ = z * _cellSize;
var cellBoundary = _cellSize - 1;
var metrics = new RoadCellLong()
{
Avg = (ushort)((
_heightMap[mapX + cellBoundary, mapZ] +
_heightMap[mapX + cellBoundary, mapZ + cellBoundary] +
_heightMap[mapX, mapZ + cellBoundary] +
_heightMap[mapX, mapZ]) / 4f),
X = x,
Z = z
};
return(metrics);
}
private IEnumerable <RoadCellLong> Get8Neighbours(RoadCellLong fromCell)
{
var neighbours = new List <RoadCellLong>();
if (fromCell.X > 0)
{
neighbours.Add(_map[fromCell.X - 1, fromCell.Z]);
if (fromCell.Z > 0)
{
neighbours.Add(_map[fromCell.X - 1, fromCell.Z - 1]);
}
if (fromCell.Z < _size - 1)
{
neighbours.Add(_map[fromCell.X - 1, fromCell.Z + 1]);
}
}
if (fromCell.X < _size - 1)
{
neighbours.Add(_map[fromCell.X + 1, fromCell.Z]);
if (fromCell.Z > 0)
{
neighbours.Add(_map[fromCell.X + 1, fromCell.Z - 1]);
}
if (fromCell.Z < _size - 1)
{
neighbours.Add(_map[fromCell.X + 1, fromCell.Z + 1]);
}
}
if (fromCell.Z > 0)
{
neighbours.Add(_map[fromCell.X, fromCell.Z - 1]);
}
if (fromCell.Z < _size - 1)
{
neighbours.Add(_map[fromCell.X, fromCell.Z + 1]);
}
return(neighbours);
}
public async Task PathFindingUsingLinkedCells(
RoadCellLong waypoint
)
{
try
{
if (waypoint.CostFromOrigin > _maximumCost)
{
_logger.Info("Too Costly - Bailing out");
}
else if (GetDirectLineDistance(waypoint, _destination) < distanceFactor)
{
ConsiderRoute(waypoint, _destination);
}
else
{
var neighbors = _getNeighbours(waypoint).ToList();
foreach (var neighbor in neighbors)
{
if (neighbor != null)
{
if (ConsiderRoute(waypoint, neighbor))
{
await PathFindingUsingLinkedCells(neighbor);
}
}
else
{
_logger.Info("Neighbor is null?");
}
}
}
}
catch (Exception exp)
{
_logger.Error(exp);
throw;
}
}
private ulong WalkDirectEstimate(RoadCellLong arg, RoadCellLong destination, Func <RoadCellLong, RoadCellLong, ulong> getRouteCost)
{
try
{
var walker = new Vector2 <int>(arg.X, arg.Z);
var previousCell = arg;
var factors = GetWalkerFactors(arg, destination);
ulong totalWalkingCost = 0;
int stepCounter = 1;
do
{
walker.X = previousCell.X + (int)Math.Round(factors.X * stepCounter, 0);
walker.Z = previousCell.Z + (int)Math.Round(factors.Z * stepCounter, 0);
totalWalkingCost += getRouteCost(previousCell, _map[walker.X, walker.Z]);
stepCounter++;
} while (walker.X != destination.X && walker.Z != destination.Z);
return(totalWalkingCost);
}
catch (Exception exp)
{
_logger.Error(exp);
}
return(long.MaxValue);
}
private IEnumerable <RoadCellLong> Get12RadiusNeighbours(RoadCellLong origin)
{
return(GetRadiusNeighbours(origin, PathFinding.TwelveRadius16Points, 23));
}
