public Map2D <TNode> SectorAt(int x, int y)
{
var sectorPosition = new SectorPosition(x / _sectorWidth, y / _sectorHeight);
if (!_sectors.TryGetValue(sectorPosition, out var sector))
{
sector = new Map2D <TNode>(_sectorWidth, _sectorHeight);
_sectors[sectorPosition] = sector;
_onSectorCreated?.Invoke(sector, sectorPosition.X * _sectorWidth, sectorPosition.Y * _sectorHeight);
}
return(sector);
}
private static bool IsOnNonBlocking <TNode>(Edge edge, Map2D <TNode> map, Func <TNode, bool> isBlocking)
{
if (edge.Start.X < 0 || edge.Start.X >= map.Width || edge.Start.Y < 0 || edge.Start.Y >= map.Height)
{
return(false);
}
if (edge.End.X < 0 || edge.End.X >= map.Width || edge.End.Y < 0 || edge.End.Y >= map.Height)
{
return(false);
}
return(!isBlocking(map[(int)edge.Start.X, (int)edge.Start.Y]) && !isBlocking(map[(int)edge.End.X, (int)edge.End.Y]));
}
public static Result Trace <TNode>(Map2D <TNode> map, Func <TNode, int> getType)
{
var blobs = new List <Blob>();
var labels = new Map2D <int>(map.Width, map.Height);
var currentLabel = 1;
for (var y = 0; y < map.Height; y++)
{
for (var x = 0; x < map.Width; x++)
{
if (getType(map[x, y]) == BackgroundType)
{
continue;
}
var aboveIn = y > 0 ? getType(map[x, y - 1]) : BackgroundType;
var belowIn = y < map.Height - 1 ? getType(map[x, y + 1]) : BackgroundType;
var belowLabel = y < map.Height - 1 ? labels[x, y + 1] : -1;
if (labels[x, y] == 0 && aboveIn == BackgroundType)
{
var contour = TraceContour(labels, currentLabel, map, getType, x, y, true);
var blob = new Blob(currentLabel, contour);
blobs.Add(blob);
currentLabel++;
}
else if (belowIn == BackgroundType && belowLabel == 0)
{
var label = labels[x, y] != 0 ? labels[x, y] : labels[x - 1, y];
var contour = TraceContour(labels, label, map, getType, x, y, false);
var blob = blobs[label - 1];
blob.AddInternalContour(contour);
}
else if (labels[x, y] == 0)
{
labels[x, y] = x > 0 ? labels[x - 1, y] : 0;
}
}
}
return(new Result(blobs, labels));
}
public static IReadOnlyList <TerrainNode> Analyze <TNode>(Map2D <TNode> map, Func <TNode, bool> isBlocking, int pruneDistanceToObstacleSquared, int maxDistanceToMergeSquared)
{
var tracingResult = ComponentLabeling.Trace(map, node => isBlocking(node) ? 1 : 0);
var contours = new List <List <Point2D> >();
foreach (var blob in tracingResult.Blobs)
{
contours.AddRange(
new[] { blob.ExternalContour }.Concat(blob.InternalContours).Select(
points => DouglasPeucker.Simplify(points.ToList(), 2 * 2, DistanceSquared)));
}
var edges = new List <Edge>();
var leftBorder = new List <int>();
var rightBorder = new List <int>();
var topBorder = new List <int>();
var bottomBorder = new List <int>();
var wallsTree = new RTree <Edge>(5, 9);
using (var voronoi = new BoostVoronoi())
{
foreach (var contour in contours)
{
var startPoint = contour.First();
foreach (var point in contour.Skip(1))
{
voronoi.AddSegment((int)startPoint.X, (int)startPoint.Y, (int)point.X, (int)point.Y);
wallsTree.Insert(new Edge {
Start = startPoint, End = point
}, new RTree <Edge> .Envelope(startPoint, point));
if ((int)startPoint.X == 0 && (int)point.X == 0)
{
leftBorder.Add((int)startPoint.Y);
leftBorder.Add((int)point.Y);
}
if ((int)startPoint.X == map.Width - 1 && (int)point.X == map.Width - 1)
{
rightBorder.Add((int)startPoint.Y);
rightBorder.Add((int)point.Y);
}
if ((int)startPoint.Y == 0 && (int)point.Y == 0)
{
topBorder.Add((int)startPoint.X);
topBorder.Add((int)point.X);
}
if ((int)startPoint.Y == map.Height - 1 && (int)point.Y == map.Height - 1)
{
bottomBorder.Add((int)startPoint.X);
bottomBorder.Add((int)point.X);
}
startPoint = point;
}
}
AddVerticalBorder(voronoi, wallsTree, leftBorder.OrderBy(x => x).ToList(), 0, map.Height - 1);
AddVerticalBorder(voronoi, wallsTree, rightBorder.OrderBy(x => x).ToList(), map.Width - 1, map.Height - 1);
AddHorizontalBorder(voronoi, wallsTree, topBorder.OrderBy(x => x).ToList(), 0, map.Width - 1);
AddHorizontalBorder(voronoi, wallsTree, bottomBorder.OrderBy(x => x).ToList(), map.Height - 1, map.Width - 1);
var visitedTwins = new List <long>();
voronoi.Construct();
for (long i = 0; i < voronoi.CountEdges; i++)
{
var edge = voronoi.GetEdge(i);
if (!edge.IsPrimary || !edge.IsFinite || visitedTwins.Contains(edge.Twin))
{
continue;
}
visitedTwins.Add(edge.Twin);
var start = voronoi.GetVertex(edge.Start);
var end = voronoi.GetVertex(edge.End);
if (double.IsNaN(start.X) || double.IsNaN(start.Y) || double.IsNaN(end.X) || double.IsNaN(end.Y))
{
continue;
}
if (edges.Any(
e => (int)e.Start.X == (int)start.X && (int)e.Start.Y == (int)start.Y &&
(int)e.End.X == (int)end.X && (int)e.End.Y == (int)end.Y ||
(int)e.Start.X == (int)end.X && (int)e.Start.Y == (int)end.Y &&
(int)e.End.X == (int)start.X && (int)e.End.Y == (int)start.Y))
{
continue;
}
edges.Add(new Edge
{
Start = new Point2D((int)start.X, (int)start.Y),
End = new Point2D((int)end.X, (int)end.Y)
});
}
}
var walkableEdges = edges.Where(edge => IsOnNonBlocking(edge, map, isBlocking)).ToList();
var nodes = new List <InternalNode>();
foreach (var node in walkableEdges)
{
if (node.Start == node.End)
{
continue;
}
var startNode = nodes.Find(x => x.Point == node.Start);
if (startNode == null)
{
startNode = CreateNode(wallsTree, node.Start);
nodes.Add(startNode);
}
var endNode = nodes.Find(x => x.Point == node.End);
if (endNode == null)
{
endNode = CreateNode(wallsTree, node.End);
nodes.Add(endNode);
}
startNode.Neighbors.Add(endNode);
endNode.Neighbors.Add(startNode);
}
PruneNodes(nodes, pruneDistanceToObstacleSquared);
GetPointsOfInterest(nodes, pruneDistanceToObstacleSquared, maxDistanceToMergeSquared);
var mergedNodes = MergeNodes(nodes, maxDistanceToMergeSquared);
return(MapToTerrainNodes(mergedNodes, wallsTree));
}
public Result(IReadOnlyCollection <Blob> blobs, Map2D <int> labels)
{
Blobs = blobs;
Labels = labels;
}
private static List <Point2D> TraceContour <TNode>(IMap2D <int> labels, int currentLabel, Map2D <TNode> map, Func <TNode, int> getType, int x, int y, bool isExternal)
{
var contour = new List <Point2D>();
var index = isExternal ? 7 : 3;
var startX = x;
var startY = y;
var localX = -1;
var localY = -1;
labels[startX, startY] = currentLabel;
var done = false;
while (!done)
{
contour.Add(new Point2D(startX, startY));
int counter;
for (counter = 0; counter < 8; counter++, index = (index + 1) % 8)
{
var differencePoint = DifferencePoints[index];
var currentX = (int)(startX + differencePoint.X);
var currentY = (int)(startY + differencePoint.Y);
if (currentX < 0 || currentX >= map.Width)
{
continue;
}
if (currentY < 0 || currentY >= map.Height)
{
continue;
}
if (getType(map[currentX, currentY]) != BackgroundType)
{
labels[currentX, currentY] = currentLabel;
if (localX < 0 && localY < 0)
{
localX = currentX;
localY = currentY;
}
else
{
done = x == startX && localX == currentX && y == startY && localY == currentY;
}
startX = currentX;
startY = currentY;
break;
}
labels[currentX, currentY] = -1;
}
if (counter == 8)
{
done = true;
}
var previous = (index + 4) % 8;
index = (previous + 2) % 8;
}
return(contour);
}
