public PathfindingNode ClosestNodeInLOS(Point p)
{
PathfindingNodeManager manager = PathfindingNodeManager.GetInstance();
CustomArrayList<PathfindingNode> inRangeNodes = new CustomArrayList<PathfindingNode>();
for( int i = 0; i < manager.GetNodeCount(); i++)
{
PathfindingNode node = manager.GetNodeAt(i);
if (this.IsCollisionBetween(p, node.GetLocation()))
{
inRangeNodes.AddLast(node);
}
}
PathfindingNode closestNode = inRangeNodes.ElementAt(0);
double closestDistance = Double.MaxValue;
for( int i = 0; i < inRangeNodes.Count(); i++ ){
PathfindingNode node = inRangeNodes.ElementAt(i);
double newDistance = PathfindingUtil.GetHypoteneuseLength(closestNode.GetLocation(), node.GetLocation());
if (closestDistance > newDistance)
{
closestDistance = newDistance;
closestNode = node;
}
}
return closestNode;
}