Ejemplo n.º 1
0
static void eml_qrsolve(const emxArray_real_T *A, emxArray_real_T *B,
  emxArray_real_T *Y)
{
  emxArray_real_T *b_A;
  real_T wj;
  real_T s;
  int32_T mn;
  int32_T nmip1;
  int32_T itemp;
  emxArray_real_T *tau;
  emxArray_int32_T *jpvt;
  emxArray_real_T *work;
  int32_T m;
  int32_T n;
  int32_T b_mn;
  emxArray_real_T *vn1;
  emxArray_real_T *vn2;
  int32_T k;
  int32_T ix;
  int32_T i;
  int32_T i_i;
  int32_T nmi;
  int32_T mmi;
  real_T rankR;
  real_T y;
  boolean_T exitg1;
  uint32_T unnamed_idx_0;
  b_emxInit_real_T(&b_A, 2);
  wj = (real_T)A->size[0];
  s = (real_T)A->size[1];
  if (wj <= s) {
    s = wj;
  }

  mn = (int32_T)s - 1;
  nmip1 = b_A->size[0] * b_A->size[1];
  b_A->size[0] = A->size[0];
  b_A->size[1] = A->size[1];
  emxEnsureCapacity((emxArray__common *)b_A, nmip1, (int32_T)sizeof(real_T));
  itemp = A->size[0] * A->size[1] - 1;
  for (nmip1 = 0; nmip1 <= itemp; nmip1++) {
    b_A->data[nmip1] = A->data[nmip1];
  }

  emxInit_real_T(&tau, 1);
  emxInit_int32_T(&jpvt, 2);
  emxInit_real_T(&work, 1);
  m = A->size[0];
  n = A->size[1];
  if (m <= n) {
    b_mn = m;
  } else {
    b_mn = n;
  }

  nmip1 = tau->size[0];
  tau->size[0] = b_mn;
  emxEnsureCapacity((emxArray__common *)tau, nmip1, (int32_T)sizeof(real_T));
  eml_signed_integer_colon(n, jpvt);
  nmip1 = work->size[0];
  work->size[0] = n;
  emxEnsureCapacity((emxArray__common *)work, nmip1, (int32_T)sizeof(real_T));
  itemp = n - 1;
  for (nmip1 = 0; nmip1 <= itemp; nmip1++) {
    work->data[nmip1] = 0.0;
  }

  emxInit_real_T(&vn1, 1);
  emxInit_real_T(&vn2, 1);
  nmip1 = vn1->size[0];
  vn1->size[0] = n;
  emxEnsureCapacity((emxArray__common *)vn1, nmip1, (int32_T)sizeof(real_T));
  nmip1 = vn2->size[0];
  vn2->size[0] = vn1->size[0];
  emxEnsureCapacity((emxArray__common *)vn2, nmip1, (int32_T)sizeof(real_T));
  k = 1;
  for (ix = 0; ix + 1 <= n; ix++) {
    vn1->data[ix] = eml_xnrm2(m, A, k);
    vn2->data[ix] = vn1->data[ix];
    k += m;
  }

  for (i = 0; i + 1 <= b_mn; i++) {
    i_i = i + i * m;
    nmi = (n - i) - 1;
    mmi = (m - i) - 1;
    nmip1 = 1 + nmi;
    if (nmip1 < 1) {
      itemp = -1;
    } else {
      itemp = 0;
      if (nmip1 > 1) {
        ix = i;
        wj = fabs(vn1->data[i]);
        for (k = 2; k <= nmip1; k++) {
          ix++;
          s = fabs(vn1->data[ix]);
          if (s > wj) {
            itemp = k - 1;
            wj = s;
          }
        }
      }
    }

    nmip1 = i + itemp;
    if (nmip1 + 1 != i + 1) {
      eml_xswap(m, b_A, m * nmip1 + 1, 1, m * i + 1, 1);
      itemp = jpvt->data[nmip1];
      jpvt->data[nmip1] = jpvt->data[i];
      jpvt->data[i] = itemp;
      vn1->data[nmip1] = vn1->data[i];
      vn2->data[nmip1] = vn2->data[i];
    }

    if (i + 1 < m) {
      k = i_i + 2;
      rankR = b_A->data[i_i];
      y = 0.0;
      if (mmi + 1 <= 0) {
      } else {
        wj = eml_xnrm2(mmi, b_A, k);
        if (wj != 0.0) {
          s = rt_hypotd_snf(fabs(b_A->data[i_i]), wj);
          if (b_A->data[i_i] >= 0.0) {
            s = -s;
          }

          if (fabs(s) < 1.0020841800044864E-292) {
            nmip1 = 0;
            do {
              nmip1++;
              eml_xscal(mmi, 9.9792015476736E+291, b_A, k);
              s *= 9.9792015476736E+291;
              rankR *= 9.9792015476736E+291;
            } while (!(fabs(s) >= 1.0020841800044864E-292));

            wj = eml_xnrm2(mmi, b_A, k);
            s = rt_hypotd_snf(fabs(rankR), wj);
            if (rankR >= 0.0) {
              s = -s;
            }

            y = (s - rankR) / s;
            eml_xscal(mmi, 1.0 / (rankR - s), b_A, k);
            for (k = 1; k <= nmip1; k++) {
              s *= 1.0020841800044864E-292;
            }

            rankR = s;
          } else {
            y = (s - b_A->data[i_i]) / s;
            wj = 1.0 / (b_A->data[i_i] - s);
            eml_xscal(mmi, wj, b_A, k);
            rankR = s;
          }
        }
      }

      tau->data[i] = y;
    } else {
      wj = b_A->data[i_i];
      rankR = b_A->data[i_i];
      b_A->data[i_i] = wj;
      tau->data[i] = 0.0;
    }

    b_A->data[i_i] = rankR;
    if (i + 1 < n) {
      rankR = b_A->data[i_i];
      b_A->data[i_i] = 1.0;
      eml_matlab_zlarf(mmi + 1, nmi, i_i + 1, tau->data[i], b_A, (i + (i + 1) *
        m) + 1, m, work);
      b_A->data[i_i] = rankR;
    }

    for (ix = i + 1; ix + 1 <= n; ix++) {
      if (vn1->data[ix] != 0.0) {
        s = fabs(b_A->data[i + b_A->size[0] * ix]) / vn1->data[ix];
        y = s * s;
        s = 1.0 - s * s;
        if (1.0 - y < 0.0) {
          s = 0.0;
        }

        wj = vn1->data[ix] / vn2->data[ix];
        if (s * (wj * wj) <= 1.4901161193847656E-8) {
          if (i + 1 < m) {
            k = (i + m * ix) + 1;
            y = 0.0;
            if (mmi < 1) {
            } else if (mmi == 1) {
              y = fabs(b_A->data[k]);
            } else {
              wj = 2.2250738585072014E-308;
              itemp = k + mmi;
              while (k + 1 <= itemp) {
                s = fabs(b_A->data[k]);
                if (s > wj) {
                  rankR = wj / s;
                  y = 1.0 + y * rankR * rankR;
                  wj = s;
                } else {
                  rankR = s / wj;
                  y += rankR * rankR;
                }

                k++;
              }

              y = wj * sqrt(y);
            }

            vn1->data[ix] = y;
            vn2->data[ix] = vn1->data[ix];
          } else {
            vn1->data[ix] = 0.0;
            vn2->data[ix] = 0.0;
          }
        } else {
          vn1->data[ix] *= sqrt(s);
        }
      }
    }
  }

  emxFree_real_T(&vn2);
  emxFree_real_T(&vn1);
  emxFree_real_T(&work);
  rankR = 0.0;
  k = 0;
  exitg1 = FALSE;
  while ((exitg1 == 0U) && (k <= mn)) {
    wj = (real_T)A->size[0];
    s = (real_T)A->size[1];
    if (wj >= s) {
      s = wj;
    }

    if (fabs(b_A->data[k + b_A->size[0] * k]) <= s * fabs(b_A->data[0]) *
        2.2204460492503131E-16) {
      exitg1 = TRUE;
    } else {
      rankR++;
      k++;
    }
  }

  unnamed_idx_0 = (uint32_T)A->size[1];
  nmip1 = Y->size[0];
  Y->size[0] = (int32_T)unnamed_idx_0;
  emxEnsureCapacity((emxArray__common *)Y, nmip1, (int32_T)sizeof(real_T));
  itemp = (int32_T)unnamed_idx_0 - 1;
  for (nmip1 = 0; nmip1 <= itemp; nmip1++) {
    Y->data[nmip1] = 0.0;
  }

  for (ix = 0; ix <= mn; ix++) {
    if (tau->data[ix] != 0.0) {
      wj = B->data[ix];
      nmip1 = A->size[0] + (int32_T)(1.0 - ((1.0 + (real_T)ix) + 1.0));
      for (i = 0; i <= nmip1 - 1; i++) {
        unnamed_idx_0 = ((uint32_T)ix + (uint32_T)i) + 2U;
        wj += b_A->data[((int32_T)unnamed_idx_0 + b_A->size[0] * ix) - 1] *
          B->data[(int32_T)unnamed_idx_0 - 1];
      }

      wj *= tau->data[ix];
      if (wj != 0.0) {
        B->data[ix] -= wj;
        nmip1 = A->size[0] + (int32_T)(1.0 - ((1.0 + (real_T)ix) + 1.0));
        for (i = 0; i <= nmip1 - 1; i++) {
          unnamed_idx_0 = ((uint32_T)ix + (uint32_T)i) + 2U;
          B->data[(int32_T)unnamed_idx_0 - 1] -= b_A->data[((int32_T)
            unnamed_idx_0 + b_A->size[0] * ix) - 1] * wj;
        }
      }
    }
  }

  emxFree_real_T(&tau);
  for (i = 0; i <= (int32_T)rankR - 1; i++) {
    Y->data[jpvt->data[(int32_T)(1.0 + (real_T)i) - 1] - 1] = B->data[(int32_T)
      (1.0 + (real_T)i) - 1];
  }

  for (ix = 0; ix <= (int32_T)-(1.0 + (-1.0 - rankR)) - 1; ix++) {
    wj = rankR + -(real_T)ix;
    Y->data[jpvt->data[(int32_T)wj - 1] - 1] = eml_div(Y->data[jpvt->data
      [(int32_T)wj - 1] - 1], b_A->data[((int32_T)wj + b_A->size[0] * ((int32_T)
      wj - 1)) - 1]);
    for (i = 0; i <= (int32_T)wj - 2; i++) {
      Y->data[jpvt->data[(int32_T)(1.0 + (real_T)i) - 1] - 1] -= Y->data
        [jpvt->data[(int32_T)wj - 1] - 1] * b_A->data[((int32_T)(1.0 + (real_T)i)
        + b_A->size[0] * ((int32_T)wj - 1)) - 1];
    }
  }

  emxFree_int32_T(&jpvt);
  emxFree_real_T(&b_A);
}
Ejemplo n.º 2
0
/*
 * Arguments    : const double A[900]
 *                double S[30]
 * Return Type  : void
 */
static void eml_xgesvd(const double A[900], double S[30])
{
  double b_A[900];
  double s[30];
  double e[30];
  double work[30];
  int i;
  int q;
  double ztest0;
  int qs;
  int qjj;
  int m;
  double rt;
  double ztest;
  int iter;
  double tiny;
  double snorm;
  int32_T exitg3;
  boolean_T exitg2;
  double f;
  double varargin_1[5];
  double mtmp;
  boolean_T exitg1;
  double sqds;
  memcpy(&b_A[0], &A[0], 900U * sizeof(double));
  for (i = 0; i < 30; i++) {
    s[i] = 0.0;
    e[i] = 0.0;
    work[i] = 0.0;
  }

  for (q = 0; q < 29; q++) {
    i = q + 30 * q;
    ztest0 = eml_xnrm2(30 - q, b_A, i + 1);
    if (ztest0 > 0.0) {
      if (b_A[i] < 0.0) {
        s[q] = -ztest0;
      } else {
        s[q] = ztest0;
      }

      eml_xscal(30 - q, eml_div(1.0, s[q]), b_A, i + 1);
      b_A[i]++;
      s[q] = -s[q];
    } else {
      s[q] = 0.0;
    }

    for (qs = q + 1; qs + 1 < 31; qs++) {
      qjj = q + 30 * qs;
      if (s[q] != 0.0) {
        eml_xaxpy(30 - q, -eml_div(eml_xdotc(30 - q, b_A, i + 1, b_A, qjj + 1),
                   b_A[q + 30 * q]), i + 1, b_A, qjj + 1);
      }

      e[qs] = b_A[qjj];
    }

    if (q + 1 <= 28) {
      ztest0 = b_eml_xnrm2(29 - q, e, q + 2);
      if (ztest0 == 0.0) {
        e[q] = 0.0;
      } else {
        if (e[q + 1] < 0.0) {
          e[q] = -ztest0;
        } else {
          e[q] = ztest0;
        }

        b_eml_xscal(29 - q, eml_div(1.0, e[q]), e, q + 2);
        e[q + 1]++;
      }

      e[q] = -e[q];
      if (e[q] != 0.0) {
        for (i = q + 1; i + 1 < 31; i++) {
          work[i] = 0.0;
        }

        for (qs = q + 1; qs + 1 < 31; qs++) {
          b_eml_xaxpy(29 - q, e[qs], b_A, (q + 30 * qs) + 2, work, q + 2);
        }

        for (qs = q + 1; qs + 1 < 31; qs++) {
          c_eml_xaxpy(29 - q, eml_div(-e[qs], e[q + 1]), work, q + 2, b_A, (q +
            30 * qs) + 2);
        }
      }
    }
  }

  m = 28;
  s[29] = b_A[899];
  e[28] = b_A[898];
  e[29] = 0.0;
  for (q = 0; q < 30; q++) {
    ztest0 = e[q];
    if (s[q] != 0.0) {
      rt = fabs(s[q]);
      ztest = eml_div(s[q], rt);
      s[q] = rt;
      if (q + 1 < 30) {
        ztest0 = eml_div(e[q], ztest);
      }
    }

    if ((q + 1 < 30) && (ztest0 != 0.0)) {
      rt = fabs(ztest0);
      ztest = eml_div(rt, ztest0);
      ztest0 = rt;
      s[q + 1] *= ztest;
    }

    e[q] = ztest0;
  }

  iter = 0;
  tiny = eml_div(2.2250738585072014E-308, 2.2204460492503131E-16);
  snorm = 0.0;
  for (i = 0; i < 30; i++) {
    ztest0 = fabs(s[i]);
    ztest = fabs(e[i]);
    if ((ztest0 >= ztest) || rtIsNaN(ztest)) {
    } else {
      ztest0 = ztest;
    }

    if ((snorm >= ztest0) || rtIsNaN(ztest0)) {
    } else {
      snorm = ztest0;
    }
  }

  while ((m + 2 > 0) && (!(iter >= 75))) {
    i = m;
    do {
      exitg3 = 0;
      q = i + 1;
      if (i + 1 == 0) {
        exitg3 = 1;
      } else {
        ztest0 = fabs(e[i]);
        if ((ztest0 <= 2.2204460492503131E-16 * (fabs(s[i]) + fabs(s[i + 1]))) ||
            (ztest0 <= tiny) || ((iter > 20) && (ztest0 <=
              2.2204460492503131E-16 * snorm))) {
          e[i] = 0.0;
          exitg3 = 1;
        } else {
          i--;
        }
      }
    } while (exitg3 == 0);

    if (i + 1 == m + 1) {
      i = 4;
    } else {
      qs = m + 2;
      qjj = m + 2;
      exitg2 = false;
      while ((!exitg2) && (qjj >= i + 1)) {
        qs = qjj;
        if (qjj == i + 1) {
          exitg2 = true;
        } else {
          ztest0 = 0.0;
          if (qjj < m + 2) {
            ztest0 = fabs(e[qjj - 1]);
          }

          if (qjj > i + 2) {
            ztest0 += fabs(e[qjj - 2]);
          }

          ztest = fabs(s[qjj - 1]);
          if ((ztest <= 2.2204460492503131E-16 * ztest0) || (ztest <= tiny)) {
            s[qjj - 1] = 0.0;
            exitg2 = true;
          } else {
            qjj--;
          }
        }
      }

      if (qs == i + 1) {
        i = 3;
      } else if (qs == m + 2) {
        i = 1;
      } else {
        i = 2;
        q = qs;
      }
    }

    switch (i) {
     case 1:
      f = e[m];
      e[m] = 0.0;
      for (i = m; i + 1 >= q + 1; i--) {
        ztest0 = s[i];
        eml_xrotg(&ztest0, &f, &ztest, &rt);
        s[i] = ztest0;
        if (i + 1 > q + 1) {
          f = -rt * e[i - 1];
          e[i - 1] *= ztest;
        }
      }
      break;

     case 2:
      f = e[q - 1];
      e[q - 1] = 0.0;
      while (q + 1 <= m + 2) {
        eml_xrotg(&s[q], &f, &ztest, &rt);
        f = -rt * e[q];
        e[q] *= ztest;
        q++;
      }
      break;

     case 3:
      varargin_1[0] = fabs(s[m + 1]);
      varargin_1[1] = fabs(s[m]);
      varargin_1[2] = fabs(e[m]);
      varargin_1[3] = fabs(s[q]);
      varargin_1[4] = fabs(e[q]);
      i = 1;
      mtmp = varargin_1[0];
      if (rtIsNaN(varargin_1[0])) {
        qs = 2;
        exitg1 = false;
        while ((!exitg1) && (qs < 6)) {
          i = qs;
          if (!rtIsNaN(varargin_1[qs - 1])) {
            mtmp = varargin_1[qs - 1];
            exitg1 = true;
          } else {
            qs++;
          }
        }
      }

      if (i < 5) {
        while (i + 1 < 6) {
          if (varargin_1[i] > mtmp) {
            mtmp = varargin_1[i];
          }

          i++;
        }
      }

      f = eml_div(s[m + 1], mtmp);
      ztest0 = eml_div(s[m], mtmp);
      ztest = eml_div(e[m], mtmp);
      sqds = eml_div(s[q], mtmp);
      rt = eml_div((ztest0 + f) * (ztest0 - f) + ztest * ztest, 2.0);
      ztest0 = f * ztest;
      ztest0 *= ztest0;
      ztest = 0.0;
      if ((rt != 0.0) || (ztest0 != 0.0)) {
        ztest = sqrt(rt * rt + ztest0);
        if (rt < 0.0) {
          ztest = -ztest;
        }

        ztest = eml_div(ztest0, rt + ztest);
      }

      f = (sqds + f) * (sqds - f) + ztest;
      ztest0 = sqds * eml_div(e[q], mtmp);
      for (i = q + 1; i <= m + 1; i++) {
        eml_xrotg(&f, &ztest0, &ztest, &rt);
        if (i > q + 1) {
          e[i - 2] = f;
        }

        f = ztest * s[i - 1] + rt * e[i - 1];
        e[i - 1] = ztest * e[i - 1] - rt * s[i - 1];
        ztest0 = rt * s[i];
        s[i] *= ztest;
        s[i - 1] = f;
        eml_xrotg(&s[i - 1], &ztest0, &ztest, &rt);
        f = ztest * e[i - 1] + rt * s[i];
        s[i] = -rt * e[i - 1] + ztest * s[i];
        ztest0 = rt * e[i];
        e[i] *= ztest;
      }

      e[m] = f;
      iter++;
      break;

     default:
      if (s[q] < 0.0) {
        s[q] = -s[q];
      }

      i = q + 1;
      while ((q + 1 < 30) && (s[q] < s[i])) {
        rt = s[q];
        s[q] = s[i];
        s[i] = rt;
        q = i;
        i++;
      }

      iter = 0;
      m--;
      break;
    }
  }

  memcpy(&S[0], &s[0], 30U * sizeof(double));
}
Ejemplo n.º 3
0
static void eml_xgesvd(const real32_T A[9], real32_T U[9], real32_T S[3],
  real32_T V[9])
{
  real32_T b_A[9];
  int32_T i;
  real32_T s[3];
  real32_T e[3];
  real32_T work[3];
  real32_T Vf[9];
  int32_T q;
  int32_T qs;
  real32_T ztest0;
  int32_T ii;
  real32_T ztest;
  int32_T m;
  real32_T rt;
  int32_T iter;
  real32_T tiny;
  real32_T snorm;
  int32_T exitg3;
  boolean_T exitg2;
  real32_T sn;
  real32_T varargin_1[5];
  boolean_T exitg1;
  real32_T sqds;
  real32_T b;
  for (i = 0; i < 9; i++) {
    b_A[i] = A[i];
  }

  for (i = 0; i < 3; i++) {
    s[i] = 0.0F;
    e[i] = 0.0F;
    work[i] = 0.0F;
  }

  for (i = 0; i < 9; i++) {
    U[i] = 0.0F;
    Vf[i] = 0.0F;
  }

  for (q = 0; q < 2; q++) {
    qs = q + 3 * q;
    ztest0 = eml_xnrm2(3 - q, b_A, qs + 1);
    if (ztest0 > 0.0F) {
      if (b_A[qs] < 0.0F) {
        s[q] = -ztest0;
      } else {
        s[q] = ztest0;
      }

      eml_xscal(3 - q, b_eml_div(1.0, s[q]), b_A, qs + 1);
      b_A[qs]++;
      s[q] = -s[q];
    } else {
      s[q] = 0.0F;
    }

    for (ii = q + 1; ii + 1 < 4; ii++) {
      i = q + 3 * ii;
      if (s[q] != 0.0F) {
        ztest0 = -eml_div(eml_xdotc(3 - q, b_A, qs + 1, b_A, i + 1), b_A[q + 3 *
                          q]);
        eml_xaxpy(3 - q, ztest0, qs + 1, b_A, i + 1);
      }

      e[ii] = b_A[i];
    }

    for (ii = q; ii + 1 < 4; ii++) {
      U[ii + 3 * q] = b_A[ii + 3 * q];
    }

    if (q + 1 <= 1) {
      ztest0 = b_eml_xnrm2(2, e, 2);
      if (ztest0 == 0.0F) {
        e[0] = 0.0F;
      } else {
        if (e[1] < 0.0F) {
          ztest = -ztest0;
        } else {
          ztest = ztest0;
        }

        if (e[1] < 0.0F) {
          e[0] = -ztest0;
        } else {
          e[0] = ztest0;
        }

        b_eml_xscal(2, b_eml_div(1.0, ztest), e, 2);
        e[1]++;
      }

      e[0] = -e[0];
      if (e[0] != 0.0F) {
        for (ii = 2; ii < 4; ii++) {
          work[ii - 1] = 0.0F;
        }

        for (ii = 1; ii + 1 < 4; ii++) {
          b_eml_xaxpy(2, e[ii], b_A, 2 + 3 * ii, work, 2);
        }

        for (ii = 1; ii + 1 < 4; ii++) {
          c_eml_xaxpy(2, eml_div(-e[ii], e[1]), work, 2, b_A, 2 + 3 * ii);
        }
      }

      for (ii = 1; ii + 1 < 4; ii++) {
        Vf[ii] = e[ii];
      }
    }
  }

  m = 1;
  s[2] = b_A[8];
  e[1] = b_A[7];
  e[2] = 0.0F;
  for (ii = 0; ii < 3; ii++) {
    U[6 + ii] = 0.0F;
  }

  U[8] = 1.0F;
  for (q = 1; q > -1; q += -1) {
    qs = q + 3 * q;
    if (s[q] != 0.0F) {
      for (ii = q + 1; ii + 1 < 4; ii++) {
        i = (q + 3 * ii) + 1;
        ztest0 = -eml_div(eml_xdotc(3 - q, U, qs + 1, U, i), U[qs]);
        eml_xaxpy(3 - q, ztest0, qs + 1, U, i);
      }

      for (ii = q; ii + 1 < 4; ii++) {
        U[ii + 3 * q] = -U[ii + 3 * q];
      }

      U[qs]++;
      ii = 1;
      while (ii <= q) {
        U[3] = 0.0F;
        ii = 2;
      }
    } else {
      for (ii = 0; ii < 3; ii++) {
        U[ii + 3 * q] = 0.0F;
      }

      U[qs] = 1.0F;
    }
  }

  for (q = 2; q > -1; q += -1) {
    if ((q + 1 <= 1) && (e[0] != 0.0F)) {
      for (ii = 2; ii < 4; ii++) {
        i = 2 + 3 * (ii - 1);
        ztest0 = -eml_div(eml_xdotc(2, Vf, 2, Vf, i), Vf[1]);
        eml_xaxpy(2, ztest0, 2, Vf, i);
      }
    }

    for (ii = 0; ii < 3; ii++) {
      Vf[ii + 3 * q] = 0.0F;
    }

    Vf[q + 3 * q] = 1.0F;
  }

  for (q = 0; q < 3; q++) {
    ztest0 = e[q];
    if (s[q] != 0.0F) {
      rt = (real32_T)fabs(s[q]);
      ztest = eml_div(s[q], rt);
      s[q] = rt;
      if (q + 1 < 3) {
        ztest0 = eml_div(e[q], ztest);
      }

      eml_xscal(3, ztest, U, 3 * q + 1);
    }

    if ((q + 1 < 3) && (ztest0 != 0.0F)) {
      rt = (real32_T)fabs(ztest0);
      ztest = eml_div(rt, ztest0);
      ztest0 = rt;
      s[q + 1] *= ztest;
      eml_xscal(3, ztest, Vf, 3 * (q + 1) + 1);
    }

    e[q] = ztest0;
  }

  iter = 0;
  tiny = eml_div(1.17549435E-38F, 1.1920929E-7F);
  snorm = 0.0F;
  for (ii = 0; ii < 3; ii++) {
    ztest0 = (real32_T)fabs(s[ii]);
    ztest = (real32_T)fabs(e[ii]);
    if ((ztest0 >= ztest) || rtIsNaNF(ztest)) {
      ztest = ztest0;
    }

    if ((snorm >= ztest) || rtIsNaNF(ztest)) {
    } else {
      snorm = ztest;
    }
  }

  while ((m + 2 > 0) && (!(iter >= 75))) {
    ii = m;
    do {
      exitg3 = 0;
      q = ii + 1;
      if (ii + 1 == 0) {
        exitg3 = 1;
      } else {
        ztest0 = (real32_T)fabs(e[ii]);
        if ((ztest0 <= 1.1920929E-7F * ((real32_T)fabs(s[ii]) + (real32_T)fabs
              (s[ii + 1]))) || (ztest0 <= tiny) || ((iter > 20) && (ztest0 <=
              1.1920929E-7F * snorm))) {
          e[ii] = 0.0F;
          exitg3 = 1;
        } else {
          ii--;
        }
      }
    } while (exitg3 == 0);

    if (ii + 1 == m + 1) {
      i = 4;
    } else {
      qs = m + 2;
      i = m + 2;
      exitg2 = FALSE;
      while ((exitg2 == FALSE) && (i >= ii + 1)) {
        qs = i;
        if (i == ii + 1) {
          exitg2 = TRUE;
        } else {
          ztest0 = 0.0F;
          if (i < m + 2) {
            ztest0 = (real32_T)fabs(e[i - 1]);
          }

          if (i > ii + 2) {
            ztest0 += (real32_T)fabs(e[i - 2]);
          }

          ztest = (real32_T)fabs(s[i - 1]);
          if ((ztest <= 1.1920929E-7F * ztest0) || (ztest <= tiny)) {
            s[i - 1] = 0.0F;
            exitg2 = TRUE;
          } else {
            i--;
          }
        }
      }

      if (qs == ii + 1) {
        i = 3;
      } else if (qs == m + 2) {
        i = 1;
      } else {
        i = 2;
        q = qs;
      }
    }

    switch (i) {
     case 1:
      ztest = e[m];
      e[m] = 0.0F;
      for (ii = m; ii + 1 >= q + 1; ii--) {
        ztest0 = s[ii];
        eml_xrotg(&ztest0, &ztest, &rt, &sn);
        s[ii] = ztest0;
        if (ii + 1 > q + 1) {
          ztest = -sn * e[0];
          e[0] *= rt;
        }

        eml_xrot(Vf, 3 * ii + 1, 3 * (m + 1) + 1, rt, sn);
      }
      break;

     case 2:
      ztest = e[q - 1];
      e[q - 1] = 0.0F;
      for (ii = q; ii + 1 <= m + 2; ii++) {
        eml_xrotg(&s[ii], &ztest, &rt, &sn);
        ztest = -sn * e[ii];
        e[ii] *= rt;
        eml_xrot(U, 3 * ii + 1, 3 * (q - 1) + 1, rt, sn);
      }
      break;

     case 3:
      varargin_1[0] = (real32_T)fabs(s[m + 1]);
      varargin_1[1] = (real32_T)fabs(s[m]);
      varargin_1[2] = (real32_T)fabs(e[m]);
      varargin_1[3] = (real32_T)fabs(s[q]);
      varargin_1[4] = (real32_T)fabs(e[q]);
      i = 1;
      sn = varargin_1[0];
      if (rtIsNaNF(varargin_1[0])) {
        ii = 2;
        exitg1 = FALSE;
        while ((exitg1 == FALSE) && (ii < 6)) {
          i = ii;
          if (!rtIsNaNF(varargin_1[ii - 1])) {
            sn = varargin_1[ii - 1];
            exitg1 = TRUE;
          } else {
            ii++;
          }
        }
      }

      if (i < 5) {
        while (i + 1 < 6) {
          if (varargin_1[i] > sn) {
            sn = varargin_1[i];
          }

          i++;
        }
      }

      rt = eml_div(s[m + 1], sn);
      ztest0 = eml_div(s[m], sn);
      ztest = eml_div(e[m], sn);
      sqds = eml_div(s[q], sn);
      b = c_eml_div((ztest0 + rt) * (ztest0 - rt) + ztest * ztest, 2.0);
      ztest0 = rt * ztest;
      ztest0 *= ztest0;
      ztest = 0.0F;
      if ((b != 0.0F) || (ztest0 != 0.0F)) {
        ztest = (real32_T)sqrt(b * b + ztest0);
        if (b < 0.0F) {
          ztest = -ztest;
        }

        ztest = eml_div(ztest0, b + ztest);
      }

      ztest += (sqds + rt) * (sqds - rt);
      ztest0 = sqds * eml_div(e[q], sn);
      for (ii = q + 1; ii <= m + 1; ii++) {
        eml_xrotg(&ztest, &ztest0, &rt, &sn);
        if (ii > q + 1) {
          e[0] = ztest;
        }

        ztest = rt * s[ii - 1];
        ztest0 = sn * e[ii - 1];
        e[ii - 1] = rt * e[ii - 1] - sn * s[ii - 1];
        b = s[ii];
        s[ii] *= rt;
        eml_xrot(Vf, 3 * (ii - 1) + 1, 3 * ii + 1, rt, sn);
        s[ii - 1] = ztest + ztest0;
        ztest0 = sn * b;
        eml_xrotg(&s[ii - 1], &ztest0, &rt, &sn);
        ztest = rt * e[ii - 1] + sn * s[ii];
        s[ii] = -sn * e[ii - 1] + rt * s[ii];
        ztest0 = sn * e[ii];
        e[ii] *= rt;
        eml_xrot(U, 3 * (ii - 1) + 1, 3 * ii + 1, rt, sn);
      }

      e[m] = ztest;
      iter++;
      break;

     default:
      if (s[q] < 0.0F) {
        s[q] = -s[q];
        eml_xscal(3, -1.0F, Vf, 3 * q + 1);
      }

      i = q + 1;
      while ((q + 1 < 3) && (s[q] < s[i])) {
        rt = s[q];
        s[q] = s[i];
        s[i] = rt;
        eml_xswap(Vf, 3 * q + 1, 3 * (q + 1) + 1);
        eml_xswap(U, 3 * q + 1, 3 * (q + 1) + 1);
        q = i;
        i++;
      }

      iter = 0;
      m--;
      break;
    }
  }

  for (ii = 0; ii < 3; ii++) {
    S[ii] = s[ii];
    for (i = 0; i < 3; i++) {
      V[i + 3 * ii] = Vf[i + 3 * ii];
    }
  }
}
Ejemplo n.º 4
0
void eml_xgesvd(const float A[25], float U[25], float S[5], float V[25])
{
  float b_A[25];
  float s[5];
  float e[5];
  float work[5];
  int kase;
  int q;
  int iter;
  boolean_T apply_transform;
  float ztest0;
  int qp1jj;
  int qs;
  int m;
  float rt;
  float ztest;
  float snorm;
  int32_T exitg3;
  boolean_T exitg2;
  float f;
  float varargin_1[5];
  float mtmp;
  boolean_T exitg1;
  float sqds;
  float mtmp_inv;

  for (int k=0;k<25;k++)
    b_A[k] = A[k];
  //mem_fp_cpy(&b_A[0], &A[0], 25);
  //memcpy(&b_A[0], &A[0], 25U * sizeof(float));
  for (kase = 0; kase < 5; kase++) {
    s[kase] = 0.0F;
    e[kase] = 0.0F;
    work[kase] = 0.0F;
  }

  for (kase = 0; kase < 25; kase++) {
    U[kase] = 0.0F;
    V[kase] = 0.0F;
  }

  for (q = 0; q < 4; q++) {
    iter = q + 5 * q;
    apply_transform = false;
    ztest0 = eml_xnrm2(5 - q, b_A, iter + 1);
    if (ztest0 > 0.0F) {
      apply_transform = true;
      if (b_A[iter] < 0.0F) {
        s[q] = -ztest0;
      } else {
        s[q] = ztest0;
      }

      if (fAbs(s[q]) >= 9.86076132E-32F) {
        ztest0 = fDiv(1.0F,  s[q]);
        kase = (iter - q) + 5;
        for (qp1jj = iter; qp1jj + 1 <= kase; qp1jj++) {
          b_A[qp1jj] *= ztest0;
        }
      } else {
        kase = (iter - q) + 5;
        for (qp1jj = iter; qp1jj + 1 <= kase; qp1jj++) {
          b_A[qp1jj] = fDiv(b_A[qp1jj],s[q]);
        }
      }

      b_A[iter]++;
      s[q] = -s[q];
    } else {
      s[q] = 0.0F;
    }

    for (qs = q + 1; qs + 1 < 6; qs++) {
      kase = q + 5 * qs;
      if (apply_transform) {
        eml_xaxpy(5 - q, -(fDiv(eml_xdotc(5 - q, b_A, iter + 1, b_A, kase + 1), b_A[q + 5 * q])), iter + 1, b_A, kase + 1);
      }

      e[qs] = b_A[kase];
    }

    for (qp1jj = q; qp1jj + 1 < 6; qp1jj++) {
      U[qp1jj + 5 * q] = b_A[qp1jj + 5 * q];
    }

    if (q + 1 <= 3) {
      ztest0 = b_eml_xnrm2(4 - q, e, q + 2);
      if (ztest0 == 0.0F) {
        e[q] = 0.0F;
      } else {
        if (e[q + 1] < 0.0F) {
          e[q] = -ztest0;
        } else {
          e[q] = ztest0;
        }

        ztest0 = e[q];
        if (fAbs(e[q]) >= 9.86076132E-32F) {
          ztest0 = fDiv(1.0F, e[q]);
          for (qp1jj = q + 1; qp1jj + 1 < 6; qp1jj++) {
            e[qp1jj] *= ztest0;
          }
        } else {
          for (qp1jj = q + 1; qp1jj + 1 < 6; qp1jj++) {
            e[qp1jj] = fDiv(e[qp1jj],ztest0);
          }
        }

        e[q + 1]++;
        e[q] = -e[q];
        for (qp1jj = q + 1; qp1jj + 1 < 6; qp1jj++) {
          work[qp1jj] = 0.0F;
        }

        for (qs = q + 1; qs + 1 < 6; qs++) {
          b_eml_xaxpy(4 - q, e[qs], b_A, (q + 5 * qs) + 2, work, q + 2);
        }

        for (qs = q + 1; qs + 1 < 6; qs++) {
          c_eml_xaxpy(4 - q, -fDiv(e[qs], e[q + 1]), work, q + 2, b_A, (q + 5 * qs) +
                      2);
        }
      }

      for (qp1jj = q + 1; qp1jj + 1 < 6; qp1jj++) {
        V[qp1jj + 5 * q] = e[qp1jj];
      }
    }
  }

  m = 3;
  s[4] = b_A[24];
  e[3] = b_A[23];
  e[4] = 0.0F;
  for (qp1jj = 0; qp1jj < 5; qp1jj++) {
    U[20 + qp1jj] = 0.0F;
  }

  U[24] = 1.0F;
  for (q = 3; q > -1; q += -1) {
    iter = q + 5 * q;
    if (s[q] != 0.0F) {
      for (qs = q + 1; qs + 1 < 6; qs++) {
        kase = (q + 5 * qs) + 1;
        eml_xaxpy(5 - q, -(fDiv(eml_xdotc(5 - q, U, iter + 1, U, kase), U[iter])),
                  iter + 1, U, kase);
      }

      for (qp1jj = q; qp1jj + 1 < 6; qp1jj++) {
        U[qp1jj + 5 * q] = -U[qp1jj + 5 * q];
      }

      U[iter]++;
      for (qp1jj = 1; qp1jj <= q; qp1jj++) {
        U[(qp1jj + 5 * q) - 1] = 0.0F;
      }
    } else {
      for (qp1jj = 0; qp1jj < 5; qp1jj++) {
        U[qp1jj + 5 * q] = 0.0F;
      }

      U[iter] = 1.0F;
    }
  }

  for (q = 4; q > -1; q += -1) {
    if ((q + 1 <= 3) && (e[q] != 0.0F)) {
      kase = (q + 5 * q) + 2;
      for (qs = q + 1; qs + 1 < 6; qs++) {
        qp1jj = (q + 5 * qs) + 2;
        eml_xaxpy(4 - q, -(fDiv(eml_xdotc(4 - q, V, kase, V, qp1jj), V[kase - 1])),
                  kase, V, qp1jj);
      }
    }

    for (qp1jj = 0; qp1jj < 5; qp1jj++) {
      V[qp1jj + 5 * q] = 0.0F;
    }

    V[q + 5 * q] = 1.0F;
  }

  for (q = 0; q < 5; q++) {
    ztest0 = e[q];
    if (s[q] != 0.0F) {
      rt = fAbs(s[q]);
      ztest = fDiv(s[q], rt);
      s[q] = rt;
      if (q + 1 < 5) {
        ztest0 = fDiv(e[q], ztest);
      }

      eml_xscal(ztest, U, 1 + 5 * q);
    }

    if ((q + 1 < 5) && (ztest0 != 0.0F)) {
      rt = fAbs(ztest0);
      ztest = fDiv(rt, ztest0);
      ztest0 = rt;
      s[q + 1] *= ztest;
      eml_xscal(ztest, V, 1 + 5 * (q + 1));
    }

    e[q] = ztest0;
  }

  iter = 0;
  snorm = 0.0F;
  for (qp1jj = 0; qp1jj < 5; qp1jj++) {
    ztest0 = fAbs(s[qp1jj]);
    ztest = fAbs(e[qp1jj]);
    if ((ztest0 >= ztest) || fIsNan(ztest)) {
    } else {
      ztest0 = ztest;
    }

    if ((snorm >= ztest0) || fIsNan(ztest0)) {
    } else {
      snorm = ztest0;
    }
  }

  while ((m + 2 > 0) && (!(iter >= 75))) {
    qp1jj = m;
    do {
      exitg3 = 0;
      q = qp1jj + 1;
      if (qp1jj + 1 == 0) {
        exitg3 = 1;
      } else {
        ztest0 = fAbs(e[qp1jj]);
        if ((ztest0 <= 1.1920929E-7F * (fAbs(s[qp1jj]) + fAbs(s[qp1jj + 1]))) || (ztest0 <= 9.86076132E-32F) || ((iter > 20)
             && (ztest0 <= 1.1920929E-7F * snorm))) {
          e[qp1jj] = 0.0F;
          exitg3 = 1;
        } else {
          qp1jj--;
        }
      }
    } while (exitg3 == 0);

    if (qp1jj + 1 == m + 1) {
      kase = 4;
    } else {
      qs = m + 2;
      kase = m + 2;
      exitg2 = false;
      while ((!exitg2) && (kase >= qp1jj + 1)) {
        qs = kase;
        if (kase == qp1jj + 1) {
          exitg2 = true;
        } else {
          ztest0 = 0.0F;
          if (kase < m + 2) {
            ztest0 = fAbs(e[kase - 1]);
          }

          if (kase > qp1jj + 2) {
            ztest0 += fAbs(e[kase - 2]);
          }

          ztest = fAbs(s[kase - 1]);
          if ((ztest <= 1.1920929E-7F * ztest0) || (ztest <= 9.86076132E-32F)) {
            s[kase - 1] = 0.0F;
            exitg2 = true;
          } else {
            kase--;
          }
        }
      }

      if (qs == qp1jj + 1) {
        kase = 3;
      } else if (qs == m + 2) {
        kase = 1;
      } else {
        kase = 2;
        q = qs;
      }
    }

    switch (kase) {
     case 1:
      f = e[m];
      e[m] = 0.0F;
      for (qp1jj = m; qp1jj + 1 >= q + 1; qp1jj--) {
        ztest0 = s[qp1jj];
        eml_xrotg(&ztest0, &f, &ztest, &rt);
        s[qp1jj] = ztest0;
        if (qp1jj + 1 > q + 1) {
          f = -rt * e[qp1jj - 1];
          e[qp1jj - 1] *= ztest;
        }

        eml_xrot(V, 1 + 5 * qp1jj, 1 + 5 * (m + 1), ztest, rt);
      }
      break;

     case 2:
      f = e[q - 1];
      e[q - 1] = 0.0F;
      for (qp1jj = q; qp1jj + 1 <= m + 2; qp1jj++) {
        eml_xrotg(&s[qp1jj], &f, &ztest, &rt);
        f = -rt * e[qp1jj];
        e[qp1jj] *= ztest;
        eml_xrot(U, 1 + 5 * qp1jj, 1 + 5 * (q - 1), ztest, rt);
      }
      break;

     case 3:
      varargin_1[0] = fAbs(s[m + 1]);
      varargin_1[1] = fAbs(s[m]);
      varargin_1[2] = fAbs(e[m]);
      varargin_1[3] = fAbs(s[q]);
      varargin_1[4] = fAbs(e[q]);
      kase = 1;
      mtmp = varargin_1[0];
      if (fIsNan(varargin_1[0])) {
        qp1jj = 2;
        exitg1 = false;
        while ((!exitg1) && (qp1jj < 6)) {
          kase = qp1jj;
          if (!fIsNan(varargin_1[qp1jj - 1])) {
            mtmp = varargin_1[qp1jj - 1];
            exitg1 = true;
          } else {
            qp1jj++;
          }
        }
      }

      if (kase < 5) {
        while (kase + 1 < 6) {
          if (varargin_1[kase] > mtmp) {
            mtmp = varargin_1[kase];
          }

          kase++;
        }
      }

      mtmp_inv = fDiv(1.0F,mtmp);
      
      f = s[m + 1] * mtmp_inv;
      ztest0 = s[m] * mtmp_inv;
      ztest = e[m] * mtmp_inv;
      sqds = s[q] * mtmp_inv;
      rt = ((ztest0 + f) * (ztest0 - f) + ztest * ztest) * 0.5F;
      ztest0 = f * ztest;
      ztest0 *= ztest0;
      if ((rt != 0.0F) || (ztest0 != 0.0F)) {
        ztest = fSqrt(rt * rt + ztest0);
        if (rt < 0.0F) {
          ztest = -ztest;
        }

        ztest = fDiv(ztest0, (rt + ztest));
      } else {
        ztest = 0.0F;
      }

      f = (sqds + f) * (sqds - f) + ztest;
      ztest0 = sqds * fDiv(e[q], mtmp);
      for (qp1jj = q + 1; qp1jj <= m + 1; qp1jj++) {
        eml_xrotg(&f, &ztest0, &ztest, &rt);
        if (qp1jj > q + 1) {
          e[qp1jj - 2] = f;
        }

        f = ztest * s[qp1jj - 1] + rt * e[qp1jj - 1];
        e[qp1jj - 1] = ztest * e[qp1jj - 1] - rt * s[qp1jj - 1];
        ztest0 = rt * s[qp1jj];
        s[qp1jj] *= ztest;
        eml_xrot(V, 1 + 5 * (qp1jj - 1), 1 + 5 * qp1jj, ztest, rt);
        s[qp1jj - 1] = f;
        eml_xrotg(&s[qp1jj - 1], &ztest0, &ztest, &rt);
        f = ztest * e[qp1jj - 1] + rt * s[qp1jj];
        s[qp1jj] = -rt * e[qp1jj - 1] + ztest * s[qp1jj];
        ztest0 = rt * e[qp1jj];
        e[qp1jj] *= ztest;
        eml_xrot(U, 1 + 5 * (qp1jj - 1), 1 + 5 * qp1jj, ztest, rt);
      }

      e[m] = f;
      iter++;
      break;

     default:
      if (s[q] < 0.0F) {
        s[q] = -s[q];
        eml_xscal(-1.0F, V, 1 + 5 * q);
      }

      kase = q + 1;
      while ((q + 1 < 5) && (s[q] < s[kase])) {
        rt = s[q];
        s[q] = s[kase];
        s[kase] = rt;
        eml_xswap(V, 1 + 5 * q, 1 + 5 * (q + 1));
        eml_xswap(U, 1 + 5 * q, 1 + 5 * (q + 1));
        q = kase;
        kase++;
      }

      iter = 0;
      m--;
      break;
    }
  }

  for (qp1jj = 0; qp1jj < 5; qp1jj++) {
    S[qp1jj] = s[qp1jj];
  }
}
Ejemplo n.º 5
0
static void eml_qrsolve(const emxArray_real_T *A, emxArray_real_T *B, double Y[2],
  double *rankR)
{
  emxArray_real_T *b_A;
  int m;
  int mn;
  int i6;
  int itemp;
  int b_m;
  int b_mn;
  double tau_data[2];
  signed char jpvt[2];
  double work[2];
  int i;
  int k;
  double vn1[2];
  double vn2[2];
  int pvt;
  double smax;
  int i_i;
  int mmi;
  double temp2;
  int ix;
  int iy;
  double atmp;
  int lastv;
  int lastc;
  boolean_T exitg2;
  int32_T exitg1;
  double t;
  unsigned int b_i;
  emxInit_real_T(&b_A, 2);
  m = A->size[0] - 2;
  mn = (int)fmin(A->size[0], 2.0) - 1;
  i6 = b_A->size[0] * b_A->size[1];
  b_A->size[0] = A->size[0];
  b_A->size[1] = 2;
  emxEnsureCapacity((emxArray__common *)b_A, i6, (int)sizeof(double));
  itemp = A->size[0] * A->size[1];
  for (i6 = 0; i6 < itemp; i6++) {
    b_A->data[i6] = A->data[i6];
  }

  b_m = b_A->size[0];
  if (b_A->size[0] <= 2) {
    b_mn = b_A->size[0];
  } else {
    b_mn = 2;
  }

  for (i6 = 0; i6 < 2; i6++) {
    jpvt[i6] = (signed char)(1 + i6);
  }

  if (b_A->size[0] == 0) {
  } else {
    for (i = 0; i < 2; i++) {
      work[i] = 0.0;
    }

    k = 1;
    for (pvt = 0; pvt < 2; pvt++) {
      smax = eml_xnrm2(b_m, b_A, k);
      vn2[pvt] = smax;
      k += b_m;
      vn1[pvt] = smax;
    }

    for (i = 0; i + 1 <= b_mn; i++) {
      i_i = i + i * b_m;
      mmi = (b_m - i) - 1;
      itemp = 0;
      if (2 - i > 1) {
        smax = fabs(vn1[i]);
        k = 2;
        while (k <= 2 - i) {
          temp2 = fabs(vn1[1]);
          if (temp2 > smax) {
            itemp = 1;
            smax = temp2;
          }

          k = 3;
        }
      }

      pvt = i + itemp;
      if (pvt + 1 != i + 1) {
        ix = b_m * pvt;
        iy = b_m * i;
        for (k = 1; k <= b_m; k++) {
          smax = b_A->data[ix];
          b_A->data[ix] = b_A->data[iy];
          b_A->data[iy] = smax;
          ix++;
          iy++;
        }

        itemp = jpvt[pvt];
        jpvt[pvt] = jpvt[i];
        jpvt[i] = (signed char)itemp;
        vn1[pvt] = vn1[i];
        vn2[pvt] = vn2[i];
      }

      if (i + 1 < b_m) {
        atmp = b_A->data[i_i];
        temp2 = 0.0;
        if (1 + mmi <= 0) {
        } else {
          smax = b_eml_xnrm2(mmi, b_A, i_i + 2);
          if (smax != 0.0) {
            smax = hypot(b_A->data[i_i], smax);
            if (b_A->data[i_i] >= 0.0) {
              smax = -smax;
            }

            if (fabs(smax) < 1.0020841800044864E-292) {
              pvt = 0;
              do {
                pvt++;
                eml_xscal(mmi, 9.9792015476736E+291, b_A, i_i + 2);
                smax *= 9.9792015476736E+291;
                atmp *= 9.9792015476736E+291;
              } while (!(fabs(smax) >= 1.0020841800044864E-292));

              smax = b_eml_xnrm2(mmi, b_A, i_i + 2);
              smax = hypot(atmp, smax);
              if (atmp >= 0.0) {
                smax = -smax;
              }

              temp2 = (smax - atmp) / smax;
              eml_xscal(mmi, 1.0 / (atmp - smax), b_A, i_i + 2);
              for (k = 1; k <= pvt; k++) {
                smax *= 1.0020841800044864E-292;
              }

              atmp = smax;
            } else {
              temp2 = (smax - b_A->data[i_i]) / smax;
              eml_xscal(mmi, 1.0 / (b_A->data[i_i] - smax), b_A, i_i + 2);
              atmp = smax;
            }
          }
        }

        tau_data[i] = temp2;
      } else {
        atmp = b_A->data[i_i];
        tau_data[i] = eml_matlab_zlarfg();
      }

      b_A->data[i_i] = atmp;
      if (i + 1 < 2) {
        atmp = b_A->data[i_i];
        b_A->data[i_i] = 1.0;
        if (tau_data[0] != 0.0) {
          lastv = mmi + 1;
          itemp = i_i + mmi;
          while ((lastv > 0) && (b_A->data[itemp] == 0.0)) {
            lastv--;
            itemp--;
          }

          lastc = 1;
          exitg2 = false;
          while ((!exitg2) && (lastc > 0)) {
            itemp = b_m + 1;
            do {
              exitg1 = 0;
              if (itemp <= b_m + lastv) {
                if (b_A->data[itemp - 1] != 0.0) {
                  exitg1 = 1;
                } else {
                  itemp++;
                }
              } else {
                lastc = 0;
                exitg1 = 2;
              }
            } while (exitg1 == 0);

            if (exitg1 == 1) {
              exitg2 = true;
            }
          }
        } else {
          lastv = 0;
          lastc = 0;
        }

        if (lastv > 0) {
          if (lastc == 0) {
          } else {
            work[0] = 0.0;
            iy = 0;
            pvt = 1 + b_m;
            while ((b_m > 0) && (pvt <= b_m + 1)) {
              ix = i_i;
              smax = 0.0;
              i6 = (pvt + lastv) - 1;
              for (itemp = pvt; itemp <= i6; itemp++) {
                smax += b_A->data[itemp - 1] * b_A->data[ix];
                ix++;
              }

              work[iy] += smax;
              iy++;
              pvt += b_m;
            }
          }

          if (-tau_data[0] == 0.0) {
          } else {
            k = b_m;
            iy = 0;
            pvt = 1;
            while (pvt <= lastc) {
              if (work[iy] != 0.0) {
                smax = work[iy] * -tau_data[0];
                ix = i_i;
                i6 = lastv + k;
                for (itemp = k; itemp + 1 <= i6; itemp++) {
                  b_A->data[itemp] += b_A->data[ix] * smax;
                  ix++;
                }
              }

              iy++;
              k += b_m;
              pvt = 2;
            }
          }
        }

        b_A->data[i_i] = atmp;
      }

      pvt = i + 2;
      while (pvt < 3) {
        itemp = (i + b_m) + 1;
        if (vn1[1] != 0.0) {
          smax = fabs(b_A->data[i + b_A->size[0]]) / vn1[1];
          smax = 1.0 - smax * smax;
          if (smax < 0.0) {
            smax = 0.0;
          }

          temp2 = vn1[1] / vn2[1];
          temp2 = smax * (temp2 * temp2);
          if (temp2 <= 1.4901161193847656E-8) {
            if (i + 1 < b_m) {
              smax = 0.0;
              if (mmi < 1) {
              } else if (mmi == 1) {
                smax = fabs(b_A->data[itemp]);
              } else {
                temp2 = 2.2250738585072014E-308;
                pvt = itemp + mmi;
                while (itemp + 1 <= pvt) {
                  atmp = fabs(b_A->data[itemp]);
                  if (atmp > temp2) {
                    t = temp2 / atmp;
                    smax = 1.0 + smax * t * t;
                    temp2 = atmp;
                  } else {
                    t = atmp / temp2;
                    smax += t * t;
                  }

                  itemp++;
                }

                smax = temp2 * sqrt(smax);
              }

              vn1[1] = smax;
              vn2[1] = smax;
            } else {
              vn1[1] = 0.0;
              vn2[1] = 0.0;
            }
          } else {
            vn1[1] *= sqrt(smax);
          }
        }

        pvt = 3;
      }
    }
  }

  *rankR = 0.0;
  if (mn + 1 > 0) {
    smax = fmax(A->size[0], 2.0) * fabs(b_A->data[0]) * 2.2204460492503131E-16;
    k = 0;
    while ((k <= mn) && (!(fabs(b_A->data[k + b_A->size[0] * k]) <= smax))) {
      (*rankR)++;
      k++;
    }
  }

  for (i = 0; i < 2; i++) {
    Y[i] = 0.0;
  }

  for (pvt = 0; pvt <= mn; pvt++) {
    if (tau_data[pvt] != 0.0) {
      smax = B->data[pvt];
      i6 = m - pvt;
      for (i = 0; i <= i6; i++) {
        b_i = ((unsigned int)pvt + i) + 2U;
        smax += b_A->data[((int)b_i + b_A->size[0] * pvt) - 1] * B->data[(int)
          b_i - 1];
      }

      smax *= tau_data[pvt];
      if (smax != 0.0) {
        B->data[pvt] -= smax;
        i6 = m - pvt;
        for (i = 0; i <= i6; i++) {
          b_i = ((unsigned int)pvt + i) + 2U;
          B->data[(int)b_i - 1] -= b_A->data[((int)b_i + b_A->size[0] * pvt) - 1]
            * smax;
        }
      }
    }
  }

  for (i = 0; i <= mn; i++) {
    Y[jpvt[i] - 1] = B->data[i];
  }

  for (pvt = 0; pvt <= mn; pvt++) {
    itemp = mn - pvt;
    Y[jpvt[itemp] - 1] /= b_A->data[itemp + b_A->size[0] * itemp];
    i = 0;
    while (i <= itemp - 1) {
      Y[jpvt[0] - 1] -= Y[jpvt[itemp] - 1] * b_A->data[b_A->size[0] * itemp];
      i = 1;
    }
  }

  emxFree_real_T(&b_A);
}