int HexGridFitting::getCubeCoordinate()

in IsometricPatternMatcher/HexGridFitting.cpp [472:532]
54 lines of code
10 McCabe index (conditional complexity)

int HexGridFitting::getCubeCoordinate(const Eigen::Matrix2Xd& transferDots,
                                      int& minX, int& maxX, int& minZ,
                                      int& maxZ, Eigen::Matrix3Xi& cubeCoor,
                                      std::vector<int>& bfsProcessSeq) {
  cubeCoor.resize(3, transferDots.cols());
  cubeCoor.fill(std::numeric_limits<int>::max());  // not detected =max int
  std::queue<int> bfsQueue;
  Eigen::VectorXi processed;
  processed.setZero(transferDots.cols(), 1);

  Eigen::Matrix3Xi cubedirection;
  cubedirection.resize(3, IsometricGridDot::kNumNeighbours);
  cubedirection << -1, 0, 1, 1, 0, -1, 1, 1, 0, -1, -1, 0, 0, -1, -1, 0, 1, 1;
  // start from the center
  Eigen::Vector2d center;
  center(0) = transferDots.row(0).mean();
  center(1) = transferDots.row(1).mean();
  Eigen::VectorXi centerNeighbour;
  neighboursIdxInArea(transferDots, center, 2 * spacing_,
                      centerNeighbour);  // find a point near the center
  CHECK(centerNeighbour.rows() > 0) << "center neighbor size should be >0";
  int startIdx = centerNeighbour(0);
  // manually set transferDots
  transferDots_ = transferDots;
  searchDirectionsOnPattern_ = getDirections(startIdx);

  // get cube coordinates
  processed(startIdx) = 1;
  cubeCoor.col(startIdx) << 0, 0, 0;
  bfsQueue.push(startIdx);
  while (!bfsQueue.empty()) {
    int centerIndx = bfsQueue.front();
    bfsQueue.pop();
    for (int k = 0; k < IsometricGridDot::kNumNeighbours; k++) {
      Eigen::VectorXi Indx;
      Eigen::Vector2d possLocation =
          transferDots.col(centerIndx) + searchDirectionsOnPattern_.col(k);
      if (neighboursIdxInArea(transferDots, possLocation, perPointSearchRadius_,
                              Indx)) {
        if (processed(Indx(0)) == 0) {
          // check if the point is near the possible location
          bfsQueue.push(Indx(0));
          bfsProcessSeq.push_back(Indx(0));
          processed(Indx(0)) = 1;
          cubeCoor.col(Indx(0)) =
              cubeCoor.col(centerIndx) + cubedirection.col(k);
          if (minX > cubeCoor(0, Indx(0))) minX = cubeCoor(0, Indx(0));
          if (minZ > cubeCoor(2, Indx(0))) minZ = cubeCoor(2, Indx(0));
          if (maxX < cubeCoor(0, Indx(0))) maxX = cubeCoor(0, Indx(0));
          if (maxZ < cubeCoor(2, Indx(0))) maxZ = cubeCoor(2, Indx(0));
        }  // end if
      }    // end if
    }      // end for
  }        // end while

  if (processed.sum() < transferDots.cols())
    LOG(INFO) << fmt::format(
        "{} points are processed in BFS from total {} detected points",
        processed.sum(), transferDots.cols());
  return startIdx;
}