Plaster
New
List
Login
c++src
default
shinmera
2019.03.25 14:04:42
void createGrid(); void evaluateImplicitFunc(); void getLines(); bool callback_key_down(Viewer& viewer, unsigned char key, int modifiers); Eigen::RowVector3d spatial_min, spatial_max; int spatial_res; std::vector<std::vector<int>>spatial_index; int spatial_i(int x, int y, int z){ return z+(y*spatial_res)+(x*spatial_res*spatial_res); } void initSpatialIndex(Eigen::RowVector3d min, Eigen::RowVector3d max, int resolution){ spatial_min = min; spatial_max = max; spatial_res = resolution; int rres = 1+resolution; spatial_index = std::vector<std::vector<int>>(rres*rres*rres, std::vector<int>()); } void registerPoint(Eigen::RowVector3d point, int value){ Eigen::RowVector3d local = (point - spatial_min).array() / (spatial_max - spatial_min).array(); int x = local[0]*spatial_res; int y = local[1]*spatial_res; int z = local[2]*spatial_res; spatial_index[spatial_i(x,y,z)].push_back(value); } std::vector<int> calcPointsInReach(Eigen::RowVector3d point, float radius){ std::vector<int> vec; // Primitive full scan for(int i=P.rows(); i<constrained_points.rows(); ++i){ float distance = (constrained_points.row(i) - point).norm(); if(distance <= radius) vec.push_back(i); } return vec; } std::vector<int> getPointsInReach(Eigen::RowVector3d point, float radius){ std::vector<int> vec; // Use spatial index localised scan Eigen::RowVector3d dim = spatial_max - spatial_min; Eigen::RowVector3d local = (point - spatial_min).array() / dim.array(); int x = local[0]*spatial_res; int y = local[1]*spatial_res; int z = local[2]*spatial_res; int r = ceil(radius / dim.norm() * spatial_res)+1; for(int i=max(0,x-r); i<min(x+r,spatial_res); ++i){ for(int j=max(0,y-r); j<min(y+r,spatial_res); ++j){ for(int k=max(0,z-r); k<min(z+r,spatial_res); ++k){ for(const auto& index: spatial_index[spatial_i(i,j,k)]){ float distance = (constrained_points.row(index) - point).norm(); if(distance <= radius) vec.push_back(index); } } } } return vec; }
Raw
Annotate
Repaste