Breadth First Search Voxel Implementation Review - c++

Question

I am trying to implement a Breadth First Search (BFS) on my custom octree data structure to find the shortest path between two voxels in a 3D voxel octree. The start and end voxels are specified by their 3D positions (startVoxel and endVoxel) in the octree. The voxel_size is the size of a single voxel in the octree. The max_distance parameter specifies the maximum distance that the BFS search should cover. The function returns the path length (i.e., the number of voxels that need to be traversed) between the start and end voxels, or -1 if the end voxel is not found within the maximum distance.
As I am experiencing some issues with the positional coordinates of my voxels (i suspect some rounding errors) I am trying to use end_voxel_distancethreshold to eliminate the need to find the exact location of the end voxels but have some "buffer". The issue is if I make it too large, I return a lot of 0 distance, if I make it too small, I return some valid results but after a certain distance, I suddenly only return -1.
My question is: Is the below implementation valid or am I overlooking something that could result in this unexpected behavior? I am confident that my K-nearest neighbor is working correctly and returns the six nearest neighbors within a radius of the voxel size.
double bfs ( Custom_Octree& octree, double voxel_size, Eigen::Vector3d startVoxel, Eigen::Vector3d endVoxel, double max_distance )
{
// Threshold for checking if the current voxel is close enough to the end voxel
double end_voxel_distance_threshold = voxel_size;
// Create a queue and push the start voxel into it
std::queue< Eigen::Vector3d > q;
q.push( startVoxel );
// Create a vector to keep track of the visited voxels
std::vector< Eigen::Vector3d > visited;
visited.push_back( startVoxel );
// Initialize distance to 0
double distance = 0;
// Keep looping until the queue is empty
while( !q.empty() ) {
// Get the size of the queue
int size = q.size();
// For each voxel in the queue
while( size-- ) {
// Get the current voxel from the front of the queue
Eigen::Vector3d currentVoxel = q.front();
q.pop();
// Check if the current voxel is close enough to the end voxel
if( ( currentVoxel - endVoxel ).norm() <= end_voxel_distance_threshold )
{
// Return the distance between the start and end voxel
return distance;
}
// Get the center of the current voxel
Eigen::Vector3d current_voxel_center( currentVoxel[0], currentVoxel[1], currentVoxel[2] );
// Find the 6 nearest neighbors of the current voxel
auto neighbors = octree.k_nearest_neighbors( current_voxel_center, voxel_size, 6 );
// For each neighbor
for( auto neighbor : neighbors ) {
// Get the center of the neighbor voxel
double x = neighbor->x_center();
double y = neighbor->y_center();
double z = neighbor->z_center();
Eigen::Vector3d nb( x, y, z );
// If the neighbor has not been visited and the distance from the start voxel is within the max_distance
if( std::find( visited.begin(), visited.end(), nb ) == visited.end() && distance <= max_distance ) {
// Push the neighbor voxel into the queue
q.push( nb );
// Add the neighbor voxel to the visited vector
visited.push_back( nb );
}
}
}
// Increment the distance by the voxel size
distance += voxel_size;
}
// Return -1 if the end voxel was not found
return -1;
}