PathNode* PathGraph::findNearestNode(const Vector3& pointAlfa) { float minDistance = 160000000.f; PathNode* node = NULL; for (int i = 0; i < pathNodes.size(); ++i) { PathNode* pathNode = pathNodes.get(i); Vector3 point(pathNode->getX(), pathNode->getY(), pathNode->getZ()); float sqrDistance = pointAlfa.squaredDistanceTo(point); if (sqrDistance < minDistance) { minDistance = sqrDistance; node = pathNode; } } return node; }