void GridDepthXform::cubicGather()

in lib/DepthMapTransform.cpp [853:948]
81 lines of code
26 McCabe index (conditional complexity)

void GridDepthXform::cubicGather(
    const float srcDepth, const Vector2f& loc,
    std::vector<double*>& paramBlocks, std::vector<double>& weights) const {
  const bool spatial = (desc_.gridSize.x() > 1);
  const bool depthWise = (desc_.gridSize.z() > 1);

  const int yStride = desc_.gridSize.x();
  const int zStride = desc_.gridSize.x() * desc_.gridSize.y();

  Vector3d maxCoord = Vector3d::Zero();
  Vector3d scaledCoord = Vector3d::Zero();
  Vector3i index = Vector3i::Zero();
  Vector3d relCoord = Vector3d::Zero();

  if (spatial) {
    maxCoord.x() = std::nextafter(desc_.gridSize.x() - 1, 0.0);
    maxCoord.y() = std::nextafter(desc_.gridSize.y() - 1, 0.0);

    scaledCoord.x() = std::clamp(
        (loc.x() + 1.0) * (desc_.gridSize.x() - 1) / 2.0, 0.0, maxCoord.x());
    scaledCoord.y() = std::clamp(
        (loc.y() + 1.0) * (desc_.gridSize.y() - 1) / 2.0, 0.0, maxCoord.y());
    index.x() = static_cast<int>(scaledCoord.x());
    index.y() = static_cast<int>(scaledCoord.y());
    assert(index.x() >= 0 && index.x() < desc_.gridSize.x() - 1);
    assert(index.y() >= 0 && index.y() < desc_.gridSize.y() - 1);

    relCoord.x() = scaledCoord.x() - index.x();
    relCoord.y() = scaledCoord.y() - index.y();
  }

  if (depthWise) {
    maxCoord.z() = std::nextafter(desc_.gridSize.z() - 1, 0.0);
    double srcDisparity = 1.0 / double(srcDepth);
    scaledCoord.z() = std::clamp(
        (srcDisparity - disparityMinMax_.x()) / handleSrcDisparityInterval_,
        0.0, maxCoord.z());
    index.z() = static_cast<int>(scaledCoord.z());
    assert(index.z() >= 0 && index.z() < desc_.gridSize.z() - 1);
    relCoord.z() = scaledCoord.z() - index.z();
  }

  // Combine list of parameters and weights
  paramBlocks.clear();
  weights.clear();

  std::array<double, 4> wx;
  std::array<double, 4> wy;
  std::array<double, 4> wz;

  if (spatial) {
    cubicSpline(wx, relCoord.x());
    cubicSpline(wy, relCoord.y());
  }
  if (depthWise) {
    cubicSpline(wz, relCoord.z());
  }

  Vector3i offset0(
      (index.x() == 0 ? 1 : 0),
      (index.y() == 0 ? 1 : 0),
      (index.z() == 0 ? 1 : 0));

  Vector3i offset1(
      (spatial ? (index.x() == desc_.gridSize.x() - 2 ? 3 : 4) : 2),
      (spatial ? (index.y() == desc_.gridSize.y() - 2 ? 3 : 4) : 2),
      (depthWise ? (index.z() == desc_.gridSize.z() - 2 ? 3 : 4) : 2));

  for (int z = offset0.z(); z < offset1.z(); ++z) {
    const int pz = index.z() - 1 + z;
    for (int y = offset0.y(); y < offset1.y(); ++y) {
      const int py = index.y() - 1 + y;
      for (int x = offset0.x(); x < offset1.x(); ++x) {
        const int px = index.x() - 1 + x;
        const double* ptr = paramBlocks_[px + py * yStride + pz * zStride];
        paramBlocks.push_back(const_cast<double*>(ptr));
        weights.push_back(0.0);
      }
    }
  }

  Vector3i outStride = offset1 - offset0;

  for (int z = 0; z < (depthWise ? 4 : 1); ++z) {
    for (int y = 0; y < (spatial ? 4 : 1); ++y) {
      for (int x = 0; x < (spatial ? 4 : 1); ++x) {
        const int cx = std::clamp(x - offset0.x(), 0, outStride.x() - 1);
        const int cy = std::clamp(y - offset0.y(), 0, outStride.y() - 1);
        const int cz = std::clamp(z - offset0.z(), 0, outStride.z() - 1);
        const int outIdx =
            cx + cy * outStride.x() + cz * outStride.x() * outStride.y();
        weights[outIdx] += wx[x] * wy[y];
      }
    }
  }
}