| 38 | { |
| 39 | |
| 40 | PointIdList PointGrid::findNeighbors3d(Eigen::Vector3d pos, double radius) const |
| 41 | { |
| 42 | BOX2D extent; |
| 43 | extent.grow(pos(0), pos(1)); // Start with center point |
| 44 | extent.grow(radius); // Expand by radius |
| 45 | extent.clip(bounds()); // Clip to grid bounds. |
| 46 | |
| 47 | // Find IJ bounding box. |
| 48 | auto [imin, jmin] = toIJ(extent.minx, extent.miny); |
| 49 | auto [imax, jmax] = toIJ(extent.maxx, extent.maxy); |
| 50 | |
| 51 | PointIdList neighbors; |
| 52 | double radius2 = radius * radius; // We use square distance |
| 53 | |
| 54 | for (uint16_t i = imin; i <= imax; ++i) |
| 55 | for (uint16_t j = jmin; j <= jmax; ++j) |
| 56 | { |
| 57 | const Cell& c = cell(i, j); |
| 58 | for (PointId id : c) |
| 59 | { |
| 60 | Eigen::Vector3d pos2(m_view.getFieldAs<double>(Dimension::Id::X, id), |
| 61 | m_view.getFieldAs<double>(Dimension::Id::Y, id), |
| 62 | m_view.getFieldAs<double>(Dimension::Id::Z, id)); |
| 63 | |
| 64 | if ((pos - pos2).squaredNorm() < radius2) |
| 65 | neighbors.push_back(id); |
| 66 | } |
| 67 | } |
| 68 | |
| 69 | return neighbors; |
| 70 | } |
| 71 | |
| 72 | PointIdList PointGrid::findNeighbors(BOX2D extent) const |
| 73 | { |
no test coverage detected