private static InternalNode CreateNode(RTree <Edge> wallsTree, Point2D point)
{
var closestObstacle = wallsTree.GetKNearest(1, point, (edge, p) => Math.Sqrt(DistanceSquared(p, edge.Start, edge.End))).First().Data;
return(new InternalNode
{
Point = point,
Neighbors = new List <InternalNode>(),
MinDistanceToObstacleSquared = DistanceSquared(point, closestObstacle.Start, closestObstacle.End)
});
}
private static TerrainNode MapToTerrainNode(InternalNode internalNode, IDictionary <InternalNode, TerrainNode> mappedNodes, RTree <Edge> wallsTree)
{
if (mappedNodes.TryGetValue(internalNode, out var terrainNode))
{
return(terrainNode);
}
var neighbors = new List <TerrainNode>();
terrainNode = new TerrainNode(internalNode.Point, internalNode.Type == InternalNode.NodeType.Choke, neighbors);
if (internalNode.Type == InternalNode.NodeType.Choke)
{
var chokeEdge = wallsTree.GetKNearest(1, internalNode.Point, (edge, p) => Math.Sqrt(DistanceSquared(p, edge.Start, edge.End))).First().Data;
terrainNode.ChokeRadius = (int)(Math.Sqrt(DistanceSquared(internalNode.Point, chokeEdge.Start, chokeEdge.End)));
}
mappedNodes[internalNode] = terrainNode;
neighbors.AddRange(internalNode.Neighbors.Select(x => MapToTerrainNode(x, mappedNodes, wallsTree)));
return(terrainNode);
}