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); }