C 中的快速范围搜索实现

我将与matlab中的rangesearch实现相同的功能。问题是数据确实很大(670万3D点(。我在C 中读取成对距离计算并改进代码，但还不够快。由于计算距离矩阵正在用于此数量数据的RAM消耗，因此不适用。我把代码放在这里。让我知道是否有一种方法可以更快。数据是在第一个维度上排序的，我想确保在每个点上，第一个邻居都是要点本身。

std::vector<std::vector<long int>> rangesearch(std::vector<std::vector<float>> &data, float radius) {
    float distmat = 0;
    float xdist = 0;
    std::vector<std::vector<long int>> indices(data.size());
    //This make sure that the first neighbour of each point is itself.
    for (unsigned long int i = 0; i < data.size(); i++) {
        indices[i].push_back(i);
    }
    // instead of computing sqrt() of distance, compute the 2nd power of radius once and compare it again and again which is faster
    radius = std::pow(radius, 2);
    for (unsigned long int i = 0; i < data.size(); i++) {
        for (long int j = i + 1; j < data.size(); j++) {
            xdist = std::pow(data[i][0] - data[j][0], 2);
            distmat = xdist + std::pow(data[i][1] - data[j][1], 2) + std::pow(data[i][2] - data[j][2], 2);
            if (distmat <= radius) {
                indices[i].push_back(j);
                indices[j].push_back(i);
            }
            //This is just to make the preprocessing faster. Data should be sorted based on X cordinates.
            //Then if the distance for x cordinate is bigger than radius it means that it will be even bigger
            // for the rest of the point so there is no need to check all of them and skip the rest!
            if (xdist > radius)
                break;
        }
    }
    return indices;
}