Get points from spherical neighborhood given a voxel-sorted cloud of points

points_in_radius_voxels_concept.cpp

IProject& proj = ...;
float R = ...;
Eigen::Vector3d ref_point = ...;

IHierarchy& h = proj.GetDefHierarchy();
IHierarchy& all_clouds_h = Query(".cloud", h).Flatten().Result(); // gets flat structure of all clouds 
auto all_clouds = all_clouds_h.GetRoot().GetElements();
IData& storage = proj.GetData();

// iterate through clouds
for(auto cloud_node=all_clouds.begin(); cloud_node!=all_clouds.end(); ++cloud_node)
{
	ICloudStorage& cloud_stor = *storage.GetElement(cloud_node.GetID())->GetCloud(); // (should check for errors)
	ICloudRoutines& cloud = cloud_stor.GetRoutines();
	
	// get points container
	PointsRange points_in_radius = cloud.GetPointsInSphere(ref_point, R); // call to CubicCloud::Routines::GetPointsInSphere
	
	// Opt1. iterate through points
	for(auto point=points_in_radius.begin(); point!=points_in_radius.end(); ++point)
	{
		// get desired point props
		Eigen::Vector3f xyz = point.GetXYZ();
		Eigen::Vector3f normal = point.GetNormal();
		float prop = point.GetDataLayer(7);
		
		// get busy with data
		prop *= xyz[2] + normal[2];
		
		// commit changes
		point.SetDataLayer(7, prop);
	}
	
	// TODO: other range usage examples - Opt2+.
}



PointsRange CubicCloud::Routines::GetPointsInSphere(const Eigen::Vector3d& ref_point, double radius)
{
	// find voxel with the point
	VoxelID vid = CubicCloud::FindVoxelWithPoint(this->voxels, ref_point); // static function inside CubicCloud.dll
	if(vid == INVALID_VOXEL_ID)
		return PointsRange();
		
	// browse neighboring voxels 
	std::set<VoxelID> voxels_to_visit(1, vid), visited_voxels;
	while(!voxels_to_visit.empty())
	{
		vid = *voxels_to_visit.begin(); 
		voxels_to_visit.erase(vid);
		
		// visit all adjacent voxels
		auto neigh_ids = CubicCloud::GetAdjacentVoxels(this->voxels, vid);
		for(auto nid=neigh_ids.begin(); nid!=neigh_ids.end(); ++nid)
		{
			if(visited_voxels.count(*nid)) // been here
				continue;
				
			auto voxel = CubicCloud::GetVoxel(this->voxels, *nid);
			if(CubicCloud::InsideSphere(voxel, ref_point, radius)) // whole voxel inside the sphere
			{
				auto points = CubicCloud::GetVoxelPoints(voxel, this->buffers);
				range.Add(points.front().GetID(), points.size()); // <-- add a range of points
				
				voxels_to_visit.insert(*nid);
			}
			else if(CubicCloud::DoesIntersectSphere(voxel, ref_point, radius)) // voxel partially in sphere
			{
				auto points = CubicCloud::GetVoxelPoints(voxel, this->buffers);
				for(auto point=points.begin(); point!=points.end(); ++point)
					if(Math::Distance3D(point.GetXYZ(), ref_point) <= R)
						range.Add(point.GetID()); // <-- add a single point
				
				voxels_to_visit.insert(*nid);
			}
		}
			
		visited_voxels.insert(vid);
	}
	
	return range;
}