Нахождение ближайшей точки между линиями в трехмерном пространстве C ++

Question

Мне сложно найти алгоритм для нахождения ближайшей точки между 3D линиями в C ++

Я нашел этот код Matlab: https://it.mathworks.com/matlabcentral/fileexchange/37192-intersection-point-of-lines-in-3d-space

Любой знает реализацию этогов C ++?

ОБНОВЛЕНИЕ:

Я конвертирую код из Matlab в C ++, не оптимизировать, но работает.

cv::Point3d minDistLines(const std::vector<cv::Point3d> & vettA, const std::vector<cv::Point3d> & vettB) {

  std::vector<double> nx(vettB.size());
  std::vector<double> ny(vettB.size());
  std::vector<double> nz(vettB.size());

  for(size_t i=0; i<vettB.size(); ++i){
    nx[i] = vettB[i].x;
    ny[i] = vettB[i].y;
    nz[i] = vettB[i].z;
  }

  double SXX = 0; for(size_t i=0; i<nx.size(); ++i){ SXX += nx[i]*nx[i]-1; }
  double SYY = 0; for(size_t i=0; i<ny.size(); ++i){ SYY += ny[i]*ny[i]-1; }
  double SZZ = 0; for(size_t i=0; i<nz.size(); ++i){ SZZ += nz[i]*nz[i]-1; }

  double SXY = 0; for(size_t i=0; i<nx.size(); ++i){ SXY += nx[i]*ny[i]; }
  double SXZ = 0; for(size_t i=0; i<ny.size(); ++i){ SXZ += nx[i]*nz[i]; }
  double SYZ = 0; for(size_t i=0; i<nz.size(); ++i){ SYZ += ny[i]*nz[i]; }

  cv::Mat S = cv::Mat(cv::Size(3,3), CV_64FC1);
  S.at<double>(0,0) = SXX; S.at<double>(0,1) = SXY; S.at<double>(0,2) = SXZ;
  S.at<double>(1,0) = SXY; S.at<double>(1,1) = SYY; S.at<double>(1,2) = SYZ;
  S.at<double>(2,0) = SXZ; S.at<double>(2,1) = SYZ; S.at<double>(2,2) = SZZ;

  double CX = 0;
  double CY = 0;
  double CZ = 0;

  for(size_t i=0; i<vettA.size(); ++i) {

    double CX1 = vettA[i].x * ((nx[i]*nx[i]) - 1.0);
    double CX2 = vettA[i].y * (nx[i]*ny[i]);
    double CX3 = vettA[i].z * (nx[i]*nz[i]);

    double CY1 = vettA[i].x * (nx[i]*ny[i]);
    double CY2 = vettA[i].y * ((ny[i]*ny[i]) - 1.0);
    double CY3 = vettA[i].z * (ny[i]*nz[i]);

    double CZ1 = vettA[i].x * (nx[i]*nz[i]);
    double CZ2 = vettA[i].y * (ny[i]*nz[i]);
    double CZ3 = vettA[i].z * ((nz[i]*nz[i]) - 1.0);

    CX += CX1 + CX2 + CX3;
    CY += CY1 + CY2 + CY3;
    CZ += CZ1 + CZ2 + CZ3;

  }

  cv::Mat C = cv::Mat(cv::Size(1,3), CV_64FC1);
  C.at<double>(0,0) = CX;
  C.at<double>(1,0) = CY;
  C.at<double>(2,0) = CZ;

  cv::Mat R;
  cv::solve(S, C, R, cv::DECOMP_NORMAL);

  cv::Point3d P;
  P.x = R.at<double>(0,0);
  P.y = R.at<double>(0,1);
  P.z = R.at<double>(0,2);

  return P;

}