private GpNode GetClosestNode(GpVector3 position) { var closestDist = float.MaxValue; GpNode closestNode = null; foreach (var gpNode in m_GpGraph.Nodes) { var sqrDist = GpVector3.DistanceSqr(position, gpNode.Position); if (sqrDist < closestDist) { closestDist = sqrDist; closestNode = gpNode; } } return(closestNode); }
private void VisitNode(GpNode node, GpNode parentNode = null) { if (!node.IsVisited) { node.SetVisited(true); if (parentNode != null) { node.SetParentNode(parentNode); CreateEdge($"NodeEdge:{parentNode.Id}", parentNode.Position.ToVector3(), node.Position.ToVector3()); } foreach (var neighborNode in node.Neighbors) { VisitNode(neighborNode, node); } } }