public List <ImprovedTile> FindPath(Entity source, BlockCoordinates target, double distance)
{
try
{
//new EmptyBlockedProvider(), // Instance of: IBockedProvider
var blockAccess = new CachedBlockAccess(source.Level);
var navigator = new ImprovedTileNavigator(
new LevelNavigator(source, blockAccess, distance, _blockCache),
new BlockDiagonalNeighborProvider(blockAccess, (int)Math.Truncate(source.KnownPosition.Y), _blockCache, source), // Instance of: INeighborProvider
new FastBlockDistanceAlgorithm(_blockCache)
);
BlockCoordinates targetPos = target;
BlockCoordinates sourcePos = (BlockCoordinates)source.KnownPosition;
var from = new ImprovedTile(sourcePos.X, sourcePos.Z);
var to = new ImprovedTile(targetPos.X, targetPos.Z);
return(navigator.Navigate(from, to).ToList() ?? new List <ImprovedTile>());
}
catch (Exception e)
{
Log.Error("Navigate", e);
}
return(new List <ImprovedTile>());
}
public int Calculate(ImprovedTile from, ImprovedTile to)
{
Vector3 vFrom = GetBlock(from).Coordinates;
Vector3 vTo = GetBlock(to).Coordinates;
return((int)Vector3.Distance(vFrom, vTo));
}
public HashSet <ImprovedTile> Navigate(ImprovedTile from, ImprovedTile to)
{
var closed = new HashSet <ImprovedTile>();
var open = new HashSet <ImprovedTile> {
from
};
var path = new Dictionary <ImprovedTile, ImprovedTile>();
var gScore = new Hashtable();
//var gScore = new Dictionary<Tile, int>();
gScore[from] = 0;
var fScore = new Hashtable();
fScore[from] = Calculate(from, to);
while (open.Any())
{
var current = open
.OrderBy(c => fScore[c])
.First();
if (current == to)
{
return(new HashSet <ImprovedTile>(ReconstructPath(path, current)));
}
open.Remove(current);
closed.Add(current);
foreach (ImprovedTile neighbor in neighborProvider.GetNeighbors(current))
{
if (closed.Contains(neighbor) || blockedProvider.IsBlocked(neighbor))
{
continue;
}
var tentativeG = (int)gScore[current] + distanceAlgorithm.Calculate(current, neighbor);
if (!open.Contains(neighbor))
{
open.Add(neighbor);
}
else if (tentativeG >= (int)gScore[neighbor])
{
continue;
}
path[neighbor] = current;
gScore[neighbor] = tentativeG;
fScore[neighbor] = (int)gScore[neighbor] + Calculate(neighbor, to);
}
}
return(null);
}
public Block GetBlock(ImprovedTile tile)
{
Block block;
if (!_blockCache.TryGetValue(tile, out block))
{
return(null);
}
return(block);
}
private bool GetNextTile(out ImprovedTile next)
{
next = new ImprovedTile(0, 0);
if (_currentPath.Count == 0)
{
return(false);
}
next = _currentPath.First();
BlockCoordinates currPos = (BlockCoordinates)_entity.KnownPosition;
if ((int)next.X == currPos.X && (int)next.Y == currPos.Z)
{
_currentPath.Remove(next);
if (!GetNextTile(out next))
{
return(false);
}
}
return(true);
}
public bool IsBlocked(ImprovedTile coord)
{
Block block;
if (!_blockCache.TryGetValue(coord, out block))
{
return(true);
}
if (block.IsSolid)
{
return(true);
}
if (Math.Abs(_entity.KnownPosition.Y - block.Coordinates.Y) > _entity.Height + 3)
{
return(true);
}
Vector2 entityPos = new Vector2(_entity.KnownPosition.X, _entity.KnownPosition.Z);
Vector2 tilePos = new Vector2(coord.X, coord.Y);
if (Vector2.Distance(entityPos, tilePos) > _distance)
{
return(true);
}
BlockCoordinates blockCoordinates = block.Coordinates;
if (IsObstructed(blockCoordinates))
{
return(true);
}
return(false);
}
public IEnumerable <ImprovedTile> GetNeighbors(ImprovedTile tile)
{
Block block;
if (!_blockCache.TryGetValue(tile, out block))
{
block = _level.GetBlock(new BlockCoordinates(tile.X, _startY, tile.Y));
_blockCache.Add(tile, block);
}
List <ImprovedTile> list = new List <ImprovedTile>();
for (int index = 0; index < Neighbors.GetLength(0); ++index)
{
var item = new ImprovedTile(tile.X + Neighbors[index, 0], tile.Y + Neighbors[index, 1]);
// Check for too high steps
BlockCoordinates coord = new BlockCoordinates(item.X, block.Coordinates.Y, item.Y);
if (_level.GetBlock(coord).IsSolid)
{
Block blockUp = _level.GetBlock(coord + BlockCoordinates.Up);
if (blockUp.IsSolid)
{
// Can't jump
continue;
}
if (IsObstructed(blockUp.Coordinates))
{
continue;
}
_blockCache[item] = blockUp;
}
else
{
var blockDown = _level.GetBlock(coord + BlockCoordinates.Down);
if (!blockDown.IsSolid)
{
if (!_level.GetBlock(coord + BlockCoordinates.Down + BlockCoordinates.Down).IsSolid)
{
// Will fall
continue;
}
if (IsObstructed(blockDown.Coordinates))
{
continue;
}
_blockCache[item] = blockDown;
}
else
{
if (IsObstructed(coord))
{
continue;
}
_blockCache[item] = _level.GetBlock(coord);
}
}
list.Add(item);
}
CheckDiagonals(block, list);
return(list);
}
private IEnumerable <ImprovedTile> ReconstructPath(IDictionary <ImprovedTile, ImprovedTile> path, ImprovedTile current)
{
List <ImprovedTile> tileList = new List <ImprovedTile>
{
current
};
while (path.ContainsKey(current))
{
current = path[current];
tileList.Add(current);
}
tileList.Reverse();
tileList.RemoveAt(0);
return(tileList);
}
private int Calculate(ImprovedTile from, ImprovedTile to)
{
return(Math.Abs(from.X - to.X) + Math.Abs(from.Y - to.Y));
}