void GridDepthXform::linearGather()

in lib/DepthMapTransform.cpp [739:851]
97 lines of code
10 McCabe index (conditional complexity)

void GridDepthXform::linearGather(
    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);

  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();
  }

  paramBlocks.clear();
  weights.clear();

  if (spatial && depthWise) {
    const int yStride = desc_.gridSize.x();
    const int zStride = desc_.gridSize.x() * desc_.gridSize.y();
    const int i0 =
        index.x() + index.y() * yStride + index.z() * zStride;
    const int i1 =
        (index.x() + 1) + index.y() * yStride + index.z() * zStride;
    const int i2 =
        index.x() + (index.y() + 1) * yStride + index.z() * zStride;
    const int i3 =
        (index.x() + 1) + (index.y() + 1) * yStride + index.z() * zStride;
    const int i4 =
        index.x() + index.y() * yStride + (index.z() + 1) * zStride;
    const int i5 =
        (index.x() + 1) + index.y() * yStride + (index.z() + 1) * zStride;
    const int i6 =
        index.x() + (index.y() + 1) * yStride + (index.z() + 1) * zStride;
    const int i7 =
        (index.x() + 1) + (index.y() + 1) * yStride + (index.z() + 1) * zStride;

    double* b0 = const_cast<double*>(&params_[i0]);
    double* b1 = const_cast<double*>(&params_[i1]);
    double* b2 = const_cast<double*>(&params_[i2]);
    double* b3 = const_cast<double*>(&params_[i3]);
    double* b4 = const_cast<double*>(&params_[i4]);
    double* b5 = const_cast<double*>(&params_[i5]);
    double* b6 = const_cast<double*>(&params_[i6]);
    double* b7 = const_cast<double*>(&params_[i7]);

    double w0 = (1.0-relCoord.x()) * (1.0-relCoord.y()) * (1.0-relCoord.z());
    double w1 = relCoord.x() * (1.0 - relCoord.y()) * (1.0 - relCoord.z());
    double w2 = (1.0 - relCoord.x()) * relCoord.y() * (1.0 - relCoord.z());
    double w3 = relCoord.x() * relCoord.y() * (1.0 - relCoord.z());
    double w4 = (1.0 - relCoord.x()) * (1.0 - relCoord.y()) * relCoord.z();
    double w5 = relCoord.x() * (1.0 - relCoord.y()) * relCoord.z();
    double w6 = (1.0 - relCoord.x()) * relCoord.y() * relCoord.z();
    double w7 = relCoord.x() * relCoord.y() * relCoord.z();

    paramBlocks = {b0, b1, b2, b3, b4, b5, b6, b7};
    weights = {w0, w1, w2, w3, w4, w5, w6, w7};

  } else if (spatial) {
    const int stride = desc_.gridSize.x();
    const int i0 = index.x() + index.y() * stride;
    const int i1 = (index.x() + 1) + index.y() * stride;
    const int i2 = index.x() + (index.y() + 1) * stride;
    const int i3 = (index.x() + 1) + (index.y() + 1) * stride;

    double* b0 = const_cast<double*>(&params_[i0]);
    double* b1 = const_cast<double*>(&params_[i1]);
    double* b2 = const_cast<double*>(&params_[i2]);
    double* b3 = const_cast<double*>(&params_[i3]);

    const double w0 = (1.0 - relCoord.x()) * (1.0 - relCoord.y());
    const double w1 = relCoord.x() * (1.0 - relCoord.y());
    const double w2 = (1.0 - relCoord.x()) * relCoord.y();
    const double w3 = relCoord.x() * relCoord.y();

    paramBlocks = {b0, b1, b2, b3};
    weights = {w0, w1, w2, w3};
  } else if (depthWise) {
    double* b0 = const_cast<double*>(&params_[index.z()]);
    double* b1 = const_cast<double*>(&params_[index.z() + 1]);

    const double w0 = (1.0 - relCoord.z());
    const double w1 = relCoord.z();

    paramBlocks = {b0, b1};
    weights = {w0, w1};
  }
}