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