mock-gr/algorithms/fun9_pinv.cpp

//
// File: pinv.cpp
//
// MATLAB Coder version            : 5.2
// C/C++ source code generated on  : 24-Mar-2023 11:40:08
//

// Include Files
#include "fun9_pinv.h"
#include "rt_nonfinite.h"
#include "fun9_svd.h"
#include "coder_array.h"
#include <cmath>
#include <math.h>

// Function Definitions
//
// Arguments    : const ::coder::array<double, 2U> &A
//                ::coder::array<double, 2U> &X
// Return Type  : void
//
namespace coder {
void eml_pinv(const ::coder::array<double, 2U> &A,
              ::coder::array<double, 2U> &X)
{
  array<double, 2U> U;
  array<double, 2U> V;
  array<double, 1U> s;
  int br;
  int i;
  int m;
  int n;
  int nx;
  m = A.size(0);
  n = A.size(1);
  X.set_size(A.size(1), A.size(0));
  nx = A.size(0) * A.size(1);
  for (i = 0; i < nx; i++) {
    X[i] = 0.0;
  }
  if ((A.size(0) != 0) && (A.size(1) != 0)) {
    int ar;
    boolean_T p;
    nx = A.size(0) * A.size(1);
    p = true;
    for (ar = 0; ar < nx; ar++) {
      if ((!p) || (std::isinf(A[ar]) || std::isnan(A[ar]))) {
        p = false;
      }
    }
    if (!p) {
      X.set_size(A.size(1), A.size(0));
      nx = A.size(0) * A.size(1);
      for (i = 0; i < nx; i++) {
        X[i] = rtNaN;
      }
    } else {
      double absx;
      int r;
      internal::svd(A, U, s, V);
      absx = std::abs(s[0]);
      if ((!std::isinf(absx)) && (!std::isnan(absx))) {
        if (absx <= 2.2250738585072014E-308) {
          absx = 4.94065645841247E-324;
        } else {
          frexp(absx, &br);
          absx = std::ldexp(1.0, br - 53);
        }
      } else {
        absx = rtNaN;
      }
      absx *= static_cast<double>(A.size(0));
      r = -1;
      ar = 0;
      while ((ar <= n - 1) && (s[ar] > absx)) {
        r++;
        ar++;
      }
      if (r + 1 > 0) {
        int cr;
        int i1;
        int ic;
        nx = 1;
        for (br = 0; br <= r; br++) {
          absx = 1.0 / s[br];
          i = nx + n;
          i1 = i - 1;
          for (ar = nx; ar <= i1; ar++) {
            V[ar - 1] = absx * V[ar - 1];
          }
          nx = i;
        }
        nx = A.size(1) * (A.size(0) - 1);
        for (cr = 0; n < 0 ? cr >= nx : cr <= nx; cr += n) {
          i = cr + 1;
          i1 = cr + n;
          for (ic = i; ic <= i1; ic++) {
            X[ic - 1] = 0.0;
          }
        }
        br = 0;
        for (cr = 0; n < 0 ? cr >= nx : cr <= nx; cr += n) {
          ar = -1;
          br++;
          i = br + m * r;
          for (int ib{br}; m < 0 ? ib >= i : ib <= i; ib += m) {
            int i2;
            i1 = cr + 1;
            i2 = cr + n;
            for (ic = i1; ic <= i2; ic++) {
              X[ic - 1] = X[ic - 1] + U[ib - 1] * V[(ar + ic) - cr];
            }
            ar += n;
          }
        }
      }
    }
  }
}

} // namespace coder

//
// File trailer for pinv.cpp
//
// [EOF]
//