static void c_eml_qrsolve(const emlrtStack *sp, const emxArray_real_T *A, emxArray_real_T *B, emxArray_real_T *Y) { emxArray_real_T *b_A; emxArray_real_T *work; int32_T mn; int32_T i51; int32_T ix; emxArray_real_T *tau; emxArray_int32_T *jpvt; int32_T m; int32_T n; int32_T b_mn; emxArray_real_T *vn1; emxArray_real_T *vn2; int32_T k; boolean_T overflow; boolean_T b12; int32_T i; int32_T i_i; int32_T nmi; int32_T mmi; int32_T pvt; int32_T iy; boolean_T b13; real_T xnorm; int32_T i52; real_T atmp; real_T d16; boolean_T b14; boolean_T b_i; ptrdiff_t n_t; ptrdiff_t incx_t; double * xix0_t; boolean_T exitg1; const mxArray *y; static const int32_T iv78[2] = { 1, 8 }; const mxArray *m14; char_T cv76[8]; static const char_T cv77[8] = { '%', '%', '%', 'd', '.', '%', 'd', 'e' }; char_T cv78[14]; uint32_T unnamed_idx_0; emlrtStack st; emlrtStack b_st; emlrtStack c_st; emlrtStack d_st; emlrtStack e_st; emlrtStack f_st; emlrtStack g_st; emlrtStack h_st; st.prev = sp; st.tls = sp->tls; b_st.prev = &st; b_st.tls = st.tls; c_st.prev = &b_st; c_st.tls = b_st.tls; d_st.prev = &c_st; d_st.tls = c_st.tls; e_st.prev = &d_st; e_st.tls = d_st.tls; f_st.prev = &e_st; f_st.tls = e_st.tls; g_st.prev = &f_st; g_st.tls = f_st.tls; h_st.prev = &g_st; h_st.tls = g_st.tls; emlrtHeapReferenceStackEnterFcnR2012b(sp); emxInit_real_T(sp, &b_A, 2, &m_emlrtRTEI, true); b_emxInit_real_T(sp, &work, 1, &rb_emlrtRTEI, true); mn = (int32_T)muDoubleScalarMin(A->size[0], A->size[1]); st.site = &mc_emlrtRSI; b_st.site = &nc_emlrtRSI; c_st.site = &oc_emlrtRSI; i51 = b_A->size[0] * b_A->size[1]; b_A->size[0] = A->size[0]; b_A->size[1] = A->size[1]; emxEnsureCapacity(&c_st, (emxArray__common *)b_A, i51, (int32_T)sizeof(real_T), &m_emlrtRTEI); ix = A->size[0] * A->size[1]; for (i51 = 0; i51 < ix; i51++) { b_A->data[i51] = A->data[i51]; } b_emxInit_real_T(&c_st, &tau, 1, &m_emlrtRTEI, true); b_emxInit_int32_T(&c_st, &jpvt, 2, &m_emlrtRTEI, true); m = b_A->size[0]; n = b_A->size[1]; b_mn = muIntScalarMin_sint32(b_A->size[0], b_A->size[1]); i51 = tau->size[0]; tau->size[0] = b_mn; emxEnsureCapacity(&c_st, (emxArray__common *)tau, i51, (int32_T)sizeof(real_T), &n_emlrtRTEI); d_st.site = &mf_emlrtRSI; e_st.site = &rb_emlrtRSI; f_st.site = &sb_emlrtRSI; g_st.site = &tb_emlrtRSI; eml_signed_integer_colon(&g_st, b_A->size[1], jpvt); if ((b_A->size[0] == 0) || (b_A->size[1] == 0)) { } else { ix = b_A->size[1]; i51 = work->size[0]; work->size[0] = ix; emxEnsureCapacity(&c_st, (emxArray__common *)work, i51, (int32_T)sizeof (real_T), &m_emlrtRTEI); for (i51 = 0; i51 < ix; i51++) { work->data[i51] = 0.0; } b_emxInit_real_T(&c_st, &vn1, 1, &pb_emlrtRTEI, true); b_emxInit_real_T(&c_st, &vn2, 1, &qb_emlrtRTEI, true); d_st.site = &tc_emlrtRSI; ix = b_A->size[1]; i51 = vn1->size[0]; vn1->size[0] = ix; emxEnsureCapacity(&c_st, (emxArray__common *)vn1, i51, (int32_T)sizeof (real_T), &pb_emlrtRTEI); i51 = vn2->size[0]; vn2->size[0] = ix; emxEnsureCapacity(&c_st, (emxArray__common *)vn2, i51, (int32_T)sizeof (real_T), &qb_emlrtRTEI); k = 1; d_st.site = &nf_emlrtRSI; overflow = (b_A->size[1] > 2147483646); if (overflow) { e_st.site = &db_emlrtRSI; check_forloop_overflow_error(&e_st); } for (ix = 0; ix + 1 <= b_A->size[1]; ix++) { d_st.site = &sc_emlrtRSI; vn1->data[ix] = b_eml_xnrm2(&d_st, b_A->size[0], b_A, k); vn2->data[ix] = vn1->data[ix]; k += b_A->size[0]; } d_st.site = &rc_emlrtRSI; if (1 > b_mn) { b12 = false; } else { b12 = (b_mn > 2147483646); } if (b12) { e_st.site = &db_emlrtRSI; check_forloop_overflow_error(&e_st); } for (i = 1; i <= b_mn; i++) { i_i = (i + (i - 1) * m) - 1; nmi = n - i; mmi = m - i; d_st.site = &of_emlrtRSI; ix = eml_ixamax(&d_st, 1 + nmi, vn1, i); pvt = (i + ix) - 2; if (pvt + 1 != i) { d_st.site = &pf_emlrtRSI; e_st.site = &bc_emlrtRSI; f_st.site = &cc_emlrtRSI; ix = 1 + m * pvt; iy = 1 + m * (i - 1); g_st.site = &dc_emlrtRSI; if (1 > m) { b13 = false; } else { b13 = (m > 2147483646); } if (b13) { h_st.site = &db_emlrtRSI; check_forloop_overflow_error(&h_st); } for (k = 1; k <= m; k++) { i51 = b_A->size[0] * b_A->size[1]; xnorm = b_A->data[emlrtDynamicBoundsCheckFastR2012b(ix, 1, i51, &le_emlrtBCI, &f_st) - 1]; i51 = b_A->size[0] * b_A->size[1]; i52 = b_A->size[0] * b_A->size[1]; b_A->data[emlrtDynamicBoundsCheckFastR2012b(ix, 1, i51, &le_emlrtBCI, &f_st) - 1] = b_A->data[emlrtDynamicBoundsCheckFastR2012b(iy, 1, i52, &le_emlrtBCI, &f_st) - 1]; i51 = b_A->size[0] * b_A->size[1]; b_A->data[emlrtDynamicBoundsCheckFastR2012b(iy, 1, i51, &le_emlrtBCI, &f_st) - 1] = xnorm; ix++; iy++; } ix = jpvt->data[pvt]; jpvt->data[pvt] = jpvt->data[i - 1]; jpvt->data[i - 1] = ix; vn1->data[pvt] = vn1->data[i - 1]; vn2->data[pvt] = vn2->data[i - 1]; } if (i < m) { d_st.site = &qc_emlrtRSI; atmp = b_A->data[i_i]; d16 = 0.0; if (1 + mmi <= 0) { } else { e_st.site = &wc_emlrtRSI; xnorm = b_eml_xnrm2(&e_st, mmi, b_A, i_i + 2); if (xnorm != 0.0) { xnorm = muDoubleScalarHypot(b_A->data[i_i], xnorm); if (b_A->data[i_i] >= 0.0) { xnorm = -xnorm; } if (muDoubleScalarAbs(xnorm) < 1.0020841800044864E-292) { ix = 0; do { ix++; e_st.site = &xc_emlrtRSI; b_eml_xscal(&e_st, mmi, 9.9792015476736E+291, b_A, i_i + 2); xnorm *= 9.9792015476736E+291; atmp *= 9.9792015476736E+291; } while (!(muDoubleScalarAbs(xnorm) >= 1.0020841800044864E-292)); e_st.site = &yc_emlrtRSI; xnorm = b_eml_xnrm2(&e_st, mmi, b_A, i_i + 2); xnorm = muDoubleScalarHypot(atmp, xnorm); if (atmp >= 0.0) { xnorm = -xnorm; } d16 = (xnorm - atmp) / xnorm; e_st.site = &ad_emlrtRSI; b_eml_xscal(&e_st, mmi, 1.0 / (atmp - xnorm), b_A, i_i + 2); e_st.site = &bd_emlrtRSI; if (1 > ix) { b14 = false; } else { b14 = (ix > 2147483646); } if (b14) { f_st.site = &db_emlrtRSI; check_forloop_overflow_error(&f_st); } for (k = 1; k <= ix; k++) { xnorm *= 1.0020841800044864E-292; } atmp = xnorm; } else { d16 = (xnorm - b_A->data[i_i]) / xnorm; atmp = 1.0 / (b_A->data[i_i] - xnorm); e_st.site = &cd_emlrtRSI; b_eml_xscal(&e_st, mmi, atmp, b_A, i_i + 2); atmp = xnorm; } } } tau->data[i - 1] = d16; } else { atmp = b_A->data[i_i]; d_st.site = &pc_emlrtRSI; tau->data[i - 1] = eml_matlab_zlarfg(); } b_A->data[i_i] = atmp; if (i < n) { atmp = b_A->data[i_i]; b_A->data[i_i] = 1.0; d_st.site = &qf_emlrtRSI; eml_matlab_zlarf(&d_st, mmi + 1, nmi, i_i + 1, tau->data[i - 1], b_A, i + i * m, m, work); b_A->data[i_i] = atmp; } d_st.site = &rf_emlrtRSI; if (i + 1 > n) { b_i = false; } else { b_i = (n > 2147483646); } if (b_i) { e_st.site = &db_emlrtRSI; check_forloop_overflow_error(&e_st); } for (ix = i; ix + 1 <= n; ix++) { if (vn1->data[ix] != 0.0) { xnorm = muDoubleScalarAbs(b_A->data[(i + b_A->size[0] * ix) - 1]) / vn1->data[ix]; xnorm = 1.0 - xnorm * xnorm; if (xnorm < 0.0) { xnorm = 0.0; } atmp = vn1->data[ix] / vn2->data[ix]; atmp = xnorm * (atmp * atmp); if (atmp <= 1.4901161193847656E-8) { if (i < m) { d_st.site = &sf_emlrtRSI; e_st.site = &uc_emlrtRSI; if (mmi < 1) { xnorm = 0.0; } else { f_st.site = &vc_emlrtRSI; g_st.site = &vc_emlrtRSI; n_t = (ptrdiff_t)(mmi); g_st.site = &vc_emlrtRSI; incx_t = (ptrdiff_t)(1); i51 = b_A->size[0] * b_A->size[1]; i52 = (i + m * ix) + 1; xix0_t = (double *)(&b_A->data[emlrtDynamicBoundsCheckFastR2012b (i52, 1, i51, &vb_emlrtBCI, &f_st) - 1]); xnorm = dnrm2(&n_t, xix0_t, &incx_t); } vn1->data[ix] = xnorm; vn2->data[ix] = vn1->data[ix]; } else { vn1->data[ix] = 0.0; vn2->data[ix] = 0.0; } } else { d_st.site = &tf_emlrtRSI; vn1->data[ix] *= muDoubleScalarSqrt(xnorm); } } } } emxFree_real_T(&vn2); emxFree_real_T(&vn1); } atmp = 0.0; if (mn > 0) { xnorm = muDoubleScalarMax(A->size[0], A->size[1]) * muDoubleScalarAbs (b_A->data[0]) * 2.2204460492503131E-16; k = 0; exitg1 = false; while ((!exitg1) && (k <= mn - 1)) { if (muDoubleScalarAbs(b_A->data[k + b_A->size[0] * k]) <= xnorm) { st.site = &lc_emlrtRSI; y = NULL; m14 = emlrtCreateCharArray(2, iv78); for (i = 0; i < 8; i++) { cv76[i] = cv77[i]; } emlrtInitCharArrayR2013a(&st, 8, m14, cv76); emlrtAssign(&y, m14); b_st.site = &tg_emlrtRSI; emlrt_marshallIn(&b_st, c_sprintf(&b_st, b_sprintf(&b_st, y, emlrt_marshallOut(14.0), emlrt_marshallOut(6.0), &o_emlrtMCI), emlrt_marshallOut(xnorm), &p_emlrtMCI), "sprintf", cv78); st.site = &kc_emlrtRSI; b_eml_warning(&st, atmp, cv78); exitg1 = true; } else { atmp++; k++; } } } unnamed_idx_0 = (uint32_T)A->size[1]; i51 = Y->size[0]; Y->size[0] = (int32_T)unnamed_idx_0; emxEnsureCapacity(sp, (emxArray__common *)Y, i51, (int32_T)sizeof(real_T), &m_emlrtRTEI); ix = (int32_T)unnamed_idx_0; for (i51 = 0; i51 < ix; i51++) { Y->data[i51] = 0.0; } for (ix = 0; ix < mn; ix++) { if (tau->data[ix] != 0.0) { xnorm = B->data[ix]; i51 = A->size[0] + (int32_T)(1.0 - ((1.0 + (real_T)ix) + 1.0)); emlrtForLoopVectorCheckR2012b((1.0 + (real_T)ix) + 1.0, 1.0, A->size[0], mxDOUBLE_CLASS, i51, &ac_emlrtRTEI, sp); for (i = 0; i < i51; i++) { unnamed_idx_0 = ((uint32_T)ix + i) + 2U; xnorm += b_A->data[((int32_T)unnamed_idx_0 + b_A->size[0] * ix) - 1] * B->data[(int32_T)unnamed_idx_0 - 1]; } xnorm *= tau->data[ix]; if (xnorm != 0.0) { B->data[ix] -= xnorm; i51 = A->size[0] + (int32_T)(1.0 - ((1.0 + (real_T)ix) + 1.0)); emlrtForLoopVectorCheckR2012b((1.0 + (real_T)ix) + 1.0, 1.0, A->size[0], mxDOUBLE_CLASS, i51, &yb_emlrtRTEI, sp); for (i = 0; i < i51; i++) { unnamed_idx_0 = ((uint32_T)ix + i) + 2U; B->data[(int32_T)unnamed_idx_0 - 1] -= b_A->data[((int32_T) unnamed_idx_0 + b_A->size[0] * ix) - 1] * xnorm; } } } } emxFree_real_T(&tau); emlrtForLoopVectorCheckR2012b(1.0, 1.0, atmp, mxDOUBLE_CLASS, (int32_T)atmp, &xb_emlrtRTEI, sp); for (i = 0; i < (int32_T)atmp; i++) { Y->data[jpvt->data[i] - 1] = B->data[i]; } emlrtForLoopVectorCheckR2012b(atmp, -1.0, 1.0, mxDOUBLE_CLASS, (int32_T)-(1.0 + (-1.0 - atmp)), &wb_emlrtRTEI, sp); for (ix = 0; ix < (int32_T)-(1.0 + (-1.0 - atmp)); ix++) { xnorm = atmp + -(real_T)ix; Y->data[jpvt->data[(int32_T)xnorm - 1] - 1] = eml_div(Y->data[jpvt->data [(int32_T)xnorm - 1] - 1], b_A->data[((int32_T)xnorm + b_A->size[0] * ((int32_T)xnorm - 1)) - 1]); for (i = 0; i < (int32_T)(xnorm - 1.0); i++) { Y->data[jpvt->data[i] - 1] -= Y->data[jpvt->data[(int32_T)xnorm - 1] - 1] * b_A->data[i + b_A->size[0] * ((int32_T)xnorm - 1)]; } } emxFree_int32_T(&jpvt); emxFree_real_T(&work); emxFree_real_T(&b_A); emlrtHeapReferenceStackLeaveFcnR2012b(sp); }
// // Arguments : const double A[476] // double U[476] // double S[17] // double V[289] // Return Type : void // static void eml_xgesvd(const double A[476], double U[476], double S[17], double V[289]) { double b_A[476]; double s[17]; double e[17]; int kase; double work[28]; int q; int iter; boolean_T apply_transform; double ztest0; int qp1jj; int qs; int m; double rt; double ztest; 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], 476U * sizeof(double)); for (kase = 0; kase < 17; kase++) { s[kase] = 0.0; e[kase] = 0.0; } memset(&work[0], 0, 28U * sizeof(double)); memset(&U[0], 0, 476U * sizeof(double)); memset(&V[0], 0, 289U * sizeof(double)); for (q = 0; q < 17; q++) { iter = q + 28 * q; apply_transform = false; ztest0 = c_eml_xnrm2(28 - q, b_A, iter + 1); if (ztest0 > 0.0) { apply_transform = true; if (b_A[iter] < 0.0) { s[q] = -ztest0; } else { s[q] = ztest0; } if (fabs(s[q]) >= 1.0020841800044864E-292) { ztest0 = 1.0 / s[q]; kase = (iter - q) + 28; for (qp1jj = iter; qp1jj + 1 <= kase; qp1jj++) { b_A[qp1jj] *= ztest0; } } else { kase = (iter - q) + 28; for (qp1jj = iter; qp1jj + 1 <= kase; qp1jj++) { b_A[qp1jj] /= s[q]; } } b_A[iter]++; s[q] = -s[q]; } else { s[q] = 0.0; } for (qs = q + 1; qs + 1 < 18; qs++) { kase = q + 28 * qs; if (apply_transform) { eml_xaxpy(28 - q, -(eml_xdotc(28 - q, b_A, iter + 1, b_A, kase + 1) / b_A[q + 28 * q]), iter + 1, b_A, kase + 1); } e[qs] = b_A[kase]; } for (qp1jj = q; qp1jj + 1 < 29; qp1jj++) { U[qp1jj + 28 * q] = b_A[qp1jj + 28 * q]; } if (q + 1 <= 15) { ztest0 = d_eml_xnrm2(16 - 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; } ztest0 = e[q]; if (fabs(e[q]) >= 1.0020841800044864E-292) { ztest0 = 1.0 / e[q]; for (qp1jj = q + 1; qp1jj + 1 < 18; qp1jj++) { e[qp1jj] *= ztest0; } } else { for (qp1jj = q + 1; qp1jj + 1 < 18; qp1jj++) { e[qp1jj] /= ztest0; } } e[q + 1]++; e[q] = -e[q]; for (qp1jj = q + 1; qp1jj + 1 < 29; qp1jj++) { work[qp1jj] = 0.0; } for (qs = q + 1; qs + 1 < 18; qs++) { b_eml_xaxpy(27 - q, e[qs], b_A, (q + 28 * qs) + 2, work, q + 2); } for (qs = q + 1; qs + 1 < 18; qs++) { c_eml_xaxpy(27 - q, -e[qs] / e[q + 1], work, q + 2, b_A, (q + 28 * qs) + 2); } } for (qp1jj = q + 1; qp1jj + 1 < 18; qp1jj++) { V[qp1jj + 17 * q] = e[qp1jj]; } } } m = 15; e[15] = b_A[463]; e[16] = 0.0; for (q = 16; q > -1; q += -1) { iter = q + 28 * q; if (s[q] != 0.0) { for (qs = q + 1; qs + 1 < 18; qs++) { kase = (q + 28 * qs) + 1; eml_xaxpy(28 - q, -(eml_xdotc(28 - q, U, iter + 1, U, kase) / U[iter]), iter + 1, U, kase); } for (qp1jj = q; qp1jj + 1 < 29; qp1jj++) { U[qp1jj + 28 * q] = -U[qp1jj + 28 * q]; } U[iter]++; for (qp1jj = 1; qp1jj <= q; qp1jj++) { U[(qp1jj + 28 * q) - 1] = 0.0; } } else { memset(&U[28 * q], 0, 28U * sizeof(double)); U[iter] = 1.0; } } for (q = 16; q > -1; q += -1) { if ((q + 1 <= 15) && (e[q] != 0.0)) { kase = (q + 17 * q) + 2; for (qs = q + 1; qs + 1 < 18; qs++) { qp1jj = (q + 17 * qs) + 2; d_eml_xaxpy(16 - q, -(b_eml_xdotc(16 - q, V, kase, V, qp1jj) / V[kase - 1]), kase, V, qp1jj); } } memset(&V[17 * q], 0, 17U * sizeof(double)); V[q + 17 * q] = 1.0; } for (q = 0; q < 17; q++) { ztest0 = e[q]; if (s[q] != 0.0) { rt = fabs(s[q]); ztest = s[q] / rt; s[q] = rt; if (q + 1 < 17) { ztest0 = e[q] / ztest; } b_eml_xscal(ztest, U, 1 + 28 * q); } if ((q + 1 < 17) && (ztest0 != 0.0)) { rt = fabs(ztest0); ztest = rt / ztest0; ztest0 = rt; s[q + 1] *= ztest; c_eml_xscal(ztest, V, 1 + 17 * (q + 1)); } e[q] = ztest0; } iter = 0; snorm = 0.0; for (qp1jj = 0; qp1jj < 17; qp1jj++) { ztest0 = fabs(s[qp1jj]); ztest = fabs(e[qp1jj]); if ((ztest0 >= ztest) || rtIsNaN(ztest)) { } else { ztest0 = ztest; } if ((snorm >= ztest0) || rtIsNaN(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 <= 2.2204460492503131E-16 * (fabs(s[qp1jj]) + fabs(s[qp1jj + 1]))) || (ztest0 <= 1.0020841800044864E-292) || ((iter > 20) && (ztest0 <= 2.2204460492503131E-16 * snorm))) { e[qp1jj] = 0.0; 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.0; 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 <= 2.2204460492503131E-16 * ztest0) || (ztest <= 1.0020841800044864E-292)) { s[kase - 1] = 0.0; 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.0; 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 + 17 * qp1jj, 1 + 17 * (m + 1), ztest, rt); } break; case 2: f = e[q - 1]; e[q - 1] = 0.0; for (qp1jj = q; qp1jj + 1 <= m + 2; qp1jj++) { eml_xrotg(&s[qp1jj], &f, &ztest, &rt); f = -rt * e[qp1jj]; e[qp1jj] *= ztest; b_eml_xrot(U, 1 + 28 * qp1jj, 1 + 28 * (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 (rtIsNaN(varargin_1[0])) { qp1jj = 2; exitg1 = false; while ((!exitg1) && (qp1jj < 6)) { kase = qp1jj; if (!rtIsNaN(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++; } } f = s[m + 1] / mtmp; ztest0 = s[m] / mtmp; ztest = e[m] / mtmp; sqds = s[q] / mtmp; rt = ((ztest0 + f) * (ztest0 - f) + ztest * ztest) / 2.0; ztest0 = f * ztest; ztest0 *= ztest0; if ((rt != 0.0) || (ztest0 != 0.0)) { ztest = sqrt(rt * rt + ztest0); if (rt < 0.0) { ztest = -ztest; } ztest = ztest0 / (rt + ztest); } else { ztest = 0.0; } f = (sqds + f) * (sqds - f) + ztest; ztest0 = sqds * (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 + 17 * (qp1jj - 1), 1 + 17 * 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; b_eml_xrot(U, 1 + 28 * (qp1jj - 1), 1 + 28 * qp1jj, ztest, rt); } e[m] = f; iter++; break; default: if (s[q] < 0.0) { s[q] = -s[q]; c_eml_xscal(-1.0, V, 1 + 17 * q); } kase = q + 1; while ((q + 1 < 17) && (s[q] < s[kase])) { rt = s[q]; s[q] = s[kase]; s[kase] = rt; b_eml_xswap(V, 1 + 17 * q, 1 + 17 * (q + 1)); c_eml_xswap(U, 1 + 28 * q, 1 + 28 * (q + 1)); q = kase; kase++; } iter = 0; m--; break; } } memcpy(&S[0], &s[0], 17U * sizeof(double)); }
/* * 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)); }
static void eml_xgesvd(const emxArray_real_T *A, real_T S_data[10], int32_T S_size[1]) { emxArray_real_T *b_A; int32_T mm; int32_T kase; int32_T n; int32_T minnp; real_T s_data[10]; real_T e[10]; emxArray_real_T *work; int32_T nrt; int32_T nct; int32_T q; int32_T qp1; int32_T iter; real_T ztest0; int32_T qs; int32_T mm1; int32_T m; real_T ztest; real_T tiny; real_T snorm; boolean_T exitg3; boolean_T exitg2; real_T sn; real_T sm; real_T varargin_1[5]; boolean_T exitg1; real_T sqds; real_T b; emxInit_real_T(&b_A, 2); mm = b_A->size[0] * b_A->size[1]; b_A->size[0] = A->size[0]; b_A->size[1] = 10; emxEnsureCapacity((emxArray__common *)b_A, mm, (int32_T)sizeof(real_T)); kase = A->size[0] * A->size[1] - 1; for (mm = 0; mm <= kase; mm++) { b_A->data[mm] = A->data[mm]; } n = A->size[0]; if (n <= 10) { minnp = n; } else { minnp = 10; } kase = n + 1; if (kase <= 10) { } else { kase = 10; } kase--; for (mm = 0; mm <= kase; mm++) { s_data[mm] = 0.0; } memset(&e[0], 0, 10U * sizeof(real_T)); b_emxInit_real_T(&work, 1); mm = work->size[0]; work->size[0] = n; emxEnsureCapacity((emxArray__common *)work, mm, (int32_T)sizeof(real_T)); kase = n - 1; for (mm = 0; mm <= kase; mm++) { work->data[mm] = 0.0; } if (A->size[0] == 0) { } else { if (8 <= n) { nrt = 8; } else { nrt = n; } if (n < 1) { kase = 1; } else { kase = n; } nct = kase - 1; if (nct <= 10) { } else { nct = 10; } if (nct >= nrt) { mm = nct; } else { mm = nrt; } for (q = 0; q + 1 <= mm; q++) { qp1 = q + 2; kase = q + n * q; iter = n - q; if (q + 1 <= nct) { ztest0 = c_eml_xnrm2(iter, b_A, kase + 1); if (ztest0 > 0.0) { if (b_A->data[kase] < 0.0) { ztest0 = -ztest0; } s_data[q] = ztest0; eml_xscal(iter, eml_div(1.0, s_data[q]), b_A, kase + 1); b_A->data[kase]++; s_data[q] = -s_data[q]; } else { s_data[q] = 0.0; } } for (qs = qp1; qs < 11; qs++) { mm1 = q + n * (qs - 1); if ((q + 1 <= nct) && (s_data[q] != 0.0)) { ztest0 = eml_xdotc(iter, b_A, kase + 1, b_A, mm1 + 1); ztest0 = -eml_div(ztest0, b_A->data[q + b_A->size[0] * q]); eml_xaxpy(iter, ztest0, kase + 1, b_A, mm1 + 1); } e[qs - 1] = b_A->data[mm1]; } if (q + 1 <= nrt) { ztest0 = d_eml_xnrm2(9 - q, e, qp1); if (ztest0 == 0.0) { e[q] = 0.0; } else { if (e[qp1 - 1] < 0.0) { ztest0 = -ztest0; } e[q] = ztest0; b_eml_xscal(9 - q, eml_div(1.0, e[q]), e, qp1); e[qp1 - 1]++; } e[q] = -e[q]; if ((qp1 <= n) && (e[q] != 0.0)) { for (kase = qp1; kase <= n; kase++) { work->data[kase - 1] = 0.0; } for (qs = qp1; qs < 11; qs++) { b_eml_xaxpy(iter - 1, e[qs - 1], b_A, qp1 + n * (qs - 1), work, qp1); } for (qs = qp1; qs < 11; qs++) { c_eml_xaxpy(iter - 1, eml_div(-e[qs - 1], e[qp1 - 1]), work, qp1, b_A, qp1 + n * (qs - 1)); } } } } m = n + 1; if (10 <= m) { m = 10; } if (nct < 10) { s_data[nct] = b_A->data[nct + b_A->size[0] * nct]; } if (n < m) { s_data[m - 1] = 0.0; } if (nrt + 1 < m) { e[nrt] = b_A->data[nrt + b_A->size[0] * (m - 1)]; } e[m - 1] = 0.0; for (q = 0; q + 1 <= m; q++) { if (s_data[q] != 0.0) { ztest = fabs(s_data[q]); ztest0 = eml_div(s_data[q], ztest); s_data[q] = ztest; if (q + 1 < m) { e[q] = eml_div(e[q], ztest0); } } if ((q + 1 < m) && (e[q] != 0.0)) { ztest = fabs(e[q]); ztest0 = eml_div(ztest, e[q]); e[q] = ztest; s_data[q + 1] *= ztest0; } } mm = m; iter = 0; tiny = eml_div(2.2250738585072014E-308, 2.2204460492503131E-16); snorm = 0.0; for (kase = 0; kase + 1 <= m; kase++) { ztest0 = fabs(s_data[kase]); ztest = fabs(e[kase]); if ((ztest0 >= ztest) || rtIsNaN(ztest)) { ztest = ztest0; } if ((snorm >= ztest) || rtIsNaN(ztest)) { } else { snorm = ztest; } } while ((m > 0) && (!(iter >= 75))) { q = m - 1; exitg3 = FALSE; while (!((exitg3 == 1U) || (q == 0))) { ztest0 = fabs(e[q - 1]); if ((ztest0 <= 2.2204460492503131E-16 * (fabs(s_data[q - 1]) + fabs (s_data[q]))) || (ztest0 <= tiny) || ((iter > 20) && (ztest0 <= 2.2204460492503131E-16 * snorm))) { e[q - 1] = 0.0; exitg3 = TRUE; } else { q--; } } if (q == m - 1) { kase = 4; } else { qs = m; kase = m; exitg2 = FALSE; while ((exitg2 == 0U) && (kase >= q)) { qs = kase; if (kase == q) { exitg2 = TRUE; } else { ztest0 = 0.0; if (kase < m) { ztest0 = fabs(e[kase - 1]); } if (kase > q + 1) { ztest0 += fabs(e[kase - 2]); } ztest = fabs(s_data[kase - 1]); if ((ztest <= 2.2204460492503131E-16 * ztest0) || (ztest <= tiny)) { s_data[kase - 1] = 0.0; exitg2 = TRUE; } else { kase--; } } } if (qs == q) { kase = 3; } else if (qs == m) { kase = 1; } else { kase = 2; q = qs; } } switch (kase) { case 1: ztest = e[m - 2]; e[m - 2] = 0.0; for (qs = m - 1; qs >= q + 1; qs--) { ztest0 = s_data[qs - 1]; eml_xrotg(&ztest0, &ztest, &sm, &sn); s_data[qs - 1] = ztest0; if (qs > q + 1) { kase = qs - 2; ztest = -sn * e[kase]; e[kase] *= sm; } } break; case 2: kase = q - 1; ztest = e[kase]; e[kase] = 0.0; while (q + 1 <= m) { eml_xrotg(&s_data[q], &ztest, &sm, &sn); ztest = -sn * e[q]; e[q] *= sm; q++; } break; case 3: mm1 = m - 2; varargin_1[0] = fabs(s_data[m - 1]); varargin_1[1] = fabs(s_data[mm1]); varargin_1[2] = fabs(e[mm1]); varargin_1[3] = fabs(s_data[q]); varargin_1[4] = fabs(e[q]); kase = 1; sn = varargin_1[0]; if (rtIsNaN(varargin_1[0])) { qs = 2; exitg1 = FALSE; while ((exitg1 == 0U) && (qs < 6)) { kase = qs; if (!rtIsNaN(varargin_1[qs - 1])) { sn = varargin_1[qs - 1]; exitg1 = TRUE; } else { qs++; } } } if (kase < 5) { while (kase + 1 < 6) { if (varargin_1[kase] > sn) { sn = varargin_1[kase]; } kase++; } } sm = eml_div(s_data[m - 1], sn); ztest0 = eml_div(s_data[mm1], sn); ztest = eml_div(e[mm1], sn); sqds = eml_div(s_data[q], sn); b = eml_div((ztest0 + sm) * (ztest0 - sm) + ztest * ztest, 2.0); ztest0 = sm * ztest; ztest0 *= ztest0; ztest = 0.0; if ((b != 0.0) || (ztest0 != 0.0)) { ztest = sqrt(b * b + ztest0); if (b < 0.0) { ztest = -ztest; } ztest = eml_div(ztest0, b + ztest); } ztest += (sqds + sm) * (sqds - sm); ztest0 = sqds * eml_div(e[q], sn); for (qs = q; qs + 1 <= mm1 + 1; qs++) { kase = qs + 1; eml_xrotg(&ztest, &ztest0, &sm, &sn); if (qs + 1 > q + 1) { e[qs - 1] = ztest; } ztest0 = sm * s_data[qs]; ztest = sn * e[qs]; e[qs] = sm * e[qs] - sn * s_data[qs]; b = s_data[kase]; s_data[kase] *= sm; s_data[qs] = ztest0 + ztest; ztest0 = sn * b; eml_xrotg(&s_data[qs], &ztest0, &sm, &sn); ztest = sm * e[qs] + sn * s_data[kase]; s_data[kase] = -sn * e[qs] + sm * s_data[kase]; ztest0 = sn * e[kase]; e[kase] *= sm; } e[m - 2] = ztest; iter++; break; default: if (s_data[q] < 0.0) { s_data[q] = -s_data[q]; } qp1 = q + 1; while ((q + 1 < mm) && (s_data[q] < s_data[qp1])) { ztest = s_data[q]; s_data[q] = s_data[qp1]; s_data[qp1] = ztest; q = qp1; qp1++; } iter = 0; m--; break; } } } emxFree_real_T(&work); emxFree_real_T(&b_A); S_size[0] = minnp; for (qs = 0; qs + 1 <= minnp; qs++) { S_data[qs] = s_data[qs]; } }
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]; } } }
/* Function Definitions */ static void b_eml_qrsolve(const emxArray_real_T *A, emxArray_real_T *B, double Y[3], double *rankR) { emxArray_real_T *b_A; int m; int mn; int i23; int itemp; int b_m; int b_mn; double tau_data[3]; signed char jpvt[3]; double work[3]; int i; int k; double vn1[3]; double vn2[3]; int iy; double smax; int i_i; int mmi; int ix; double temp2; int pvt; double atmp; int i_ip1; 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], 3.0) - 1; i23 = b_A->size[0] * b_A->size[1]; b_A->size[0] = A->size[0]; b_A->size[1] = 3; emxEnsureCapacity((emxArray__common *)b_A, i23, (int)sizeof(double)); itemp = A->size[0] * A->size[1]; for (i23 = 0; i23 < itemp; i23++) { b_A->data[i23] = A->data[i23]; } b_m = b_A->size[0]; if (b_A->size[0] <= 3) { b_mn = b_A->size[0]; } else { b_mn = 3; } for (i23 = 0; i23 < 3; i23++) { jpvt[i23] = (signed char)(1 + i23); } if (b_A->size[0] == 0) { } else { for (i = 0; i < 3; i++) { work[i] = 0.0; } k = 1; for (iy = 0; iy < 3; iy++) { smax = c_eml_xnrm2(b_m, b_A, k); vn2[iy] = smax; k += b_m; vn1[iy] = smax; } for (i = 0; i + 1 <= b_mn; i++) { i_i = i + i * b_m; mmi = (b_m - i) - 1; itemp = 1; if (3 - i > 1) { ix = i; smax = fabs(vn1[i]); for (k = 2; k <= 3 - i; k++) { ix++; temp2 = fabs(vn1[ix]); if (temp2 > smax) { itemp = k; smax = temp2; } } } pvt = (i + itemp) - 1; 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 = d_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) { itemp = 0; do { itemp++; b_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 = d_eml_xnrm2(mmi, b_A, i_i + 2); smax = hypot(atmp, smax); if (atmp >= 0.0) { smax = -smax; } temp2 = (smax - atmp) / smax; b_eml_xscal(mmi, 1.0 / (atmp - smax), b_A, i_i + 2); for (k = 1; k <= itemp; k++) { smax *= 1.0020841800044864E-292; } atmp = smax; } else { temp2 = (smax - b_A->data[i_i]) / smax; b_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 < 3) { atmp = b_A->data[i_i]; b_A->data[i_i] = 1.0; i_ip1 = (i + (i + 1) * b_m) + 1; if (tau_data[i] != 0.0) { lastv = mmi; itemp = i_i + mmi; while ((lastv + 1 > 0) && (b_A->data[itemp] == 0.0)) { lastv--; itemp--; } lastc = 2 - i; exitg2 = false; while ((!exitg2) && (lastc > 0)) { itemp = i_ip1 + (lastc - 1) * b_m; k = itemp; do { exitg1 = 0; if (k <= itemp + lastv) { if (b_A->data[k - 1] != 0.0) { exitg1 = 1; } else { k++; } } else { lastc--; exitg1 = 2; } } while (exitg1 == 0); if (exitg1 == 1) { exitg2 = true; } } } else { lastv = -1; lastc = 0; } if (lastv + 1 > 0) { if (lastc == 0) { } else { for (iy = 1; iy <= lastc; iy++) { work[iy - 1] = 0.0; } iy = 0; i23 = i_ip1 + b_m * (lastc - 1); itemp = i_ip1; while ((b_m > 0) && (itemp <= i23)) { ix = i_i; smax = 0.0; pvt = itemp + lastv; for (k = itemp; k <= pvt; k++) { smax += b_A->data[k - 1] * b_A->data[ix]; ix++; } work[iy] += smax; iy++; itemp += b_m; } } if (-tau_data[i] == 0.0) { } else { itemp = 0; for (iy = 1; iy <= lastc; iy++) { if (work[itemp] != 0.0) { smax = work[itemp] * -tau_data[i]; ix = i_i; i23 = lastv + i_ip1; for (pvt = i_ip1; pvt <= i23; pvt++) { b_A->data[pvt - 1] += b_A->data[ix] * smax; ix++; } } itemp++; i_ip1 += b_m; } } } b_A->data[i_i] = atmp; } for (iy = i + 1; iy + 1 < 4; iy++) { itemp = (i + b_m * iy) + 1; if (vn1[iy] != 0.0) { smax = fabs(b_A->data[i + b_A->size[0] * iy]) / vn1[iy]; smax = 1.0 - smax * smax; if (smax < 0.0) { smax = 0.0; } temp2 = vn1[iy] / vn2[iy]; 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[iy] = smax; vn2[iy] = vn1[iy]; } else { vn1[iy] = 0.0; vn2[iy] = 0.0; } } else { vn1[iy] *= sqrt(smax); } } } } } *rankR = 0.0; if (mn + 1 > 0) { smax = fmax(A->size[0], 3.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 < 3; i++) { Y[i] = 0.0; } for (iy = 0; iy <= mn; iy++) { if (tau_data[iy] != 0.0) { smax = B->data[iy]; i23 = m - iy; for (i = 0; i <= i23; i++) { b_i = ((unsigned int)iy + i) + 2U; smax += b_A->data[((int)b_i + b_A->size[0] * iy) - 1] * B->data[(int)b_i - 1]; } smax *= tau_data[iy]; if (smax != 0.0) { B->data[iy] -= smax; i23 = m - iy; for (i = 0; i <= i23; i++) { b_i = ((unsigned int)iy + i) + 2U; B->data[(int)b_i - 1] -= b_A->data[((int)b_i + b_A->size[0] * iy) - 1] * smax; } } } } for (i = 0; i <= mn; i++) { Y[jpvt[i] - 1] = B->data[i]; } for (iy = 0; iy <= mn; iy++) { itemp = mn - iy; Y[jpvt[itemp] - 1] /= b_A->data[itemp + b_A->size[0] * itemp]; for (i = 0; i < itemp; i++) { Y[jpvt[i] - 1] -= Y[jpvt[itemp] - 1] * b_A->data[i + b_A->size[0] * itemp]; } } emxFree_real_T(&b_A); }