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);
}
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);
}
/*
* 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];
}
}
}
static void b_eml_xgesvd(const real32_T A[16], real32_T U[16], real32_T S[4],
real32_T V[16])
{
real32_T b_A[16];
real32_T s[4];
real32_T e[4];
real32_T work[4];
int32_T i;
real32_T Vf[16];
int32_T q;
int32_T qs;
real32_T ztest0;
int32_T ii;
int32_T m;
real32_T rt;
real32_T ztest;
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;
memcpy(&b_A[0], &A[0], sizeof(real32_T) << 4);
for (i = 0; i < 4; i++) {
s[i] = 0.0F;
e[i] = 0.0F;
work[i] = 0.0F;
}
for (i = 0; i < 16; i++) {
U[i] = 0.0F;
Vf[i] = 0.0F;
}
for (q = 0; q < 3; q++) {
qs = q + (q << 2);
ztest0 = c_eml_xnrm2(4 - q, b_A, qs + 1);
if (ztest0 > 0.0F) {
if (b_A[qs] < 0.0F) {
s[q] = -ztest0;
} else {
s[q] = ztest0;
}
c_eml_xscal(4 - 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 < 5; ii++) {
i = q + (ii << 2);
if (s[q] != 0.0F) {
ztest0 = -eml_div(b_eml_xdotc(4 - q, b_A, qs + 1, b_A, i + 1), b_A[q +
(q << 2)]);
d_eml_xaxpy(4 - q, ztest0, qs + 1, b_A, i + 1);
}
e[ii] = b_A[i];
}
for (ii = q; ii + 1 < 5; ii++) {
U[ii + (q << 2)] = b_A[ii + (q << 2)];
}
if (q + 1 <= 2) {
ztest0 = d_eml_xnrm2(3 - 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 = b_eml_div(1.0, e[q]);
d_eml_xscal(3 - q, ztest0, e, q + 2);
e[q + 1]++;
}
e[q] = -e[q];
if (e[q] != 0.0F) {
for (ii = q + 1; ii + 1 < 5; ii++) {
work[ii] = 0.0F;
}
for (ii = q + 1; ii + 1 < 5; ii++) {
e_eml_xaxpy(3 - q, e[ii], b_A, (q + (ii << 2)) + 2, work, q + 2);
}
for (ii = q + 1; ii + 1 < 5; ii++) {
f_eml_xaxpy(3 - q, eml_div(-e[ii], e[q + 1]), work, q + 2, b_A, (q +
(ii << 2)) + 2);
}
}
for (ii = q + 1; ii + 1 < 5; ii++) {
Vf[ii + (q << 2)] = e[ii];
}
}
}
m = 2;
s[3] = b_A[15];
e[2] = b_A[14];
e[3] = 0.0F;
for (ii = 0; ii < 4; ii++) {
U[12 + ii] = 0.0F;
}
U[15] = 1.0F;
for (q = 2; q > -1; q += -1) {
qs = q + (q << 2);
if (s[q] != 0.0F) {
for (ii = q + 1; ii + 1 < 5; ii++) {
i = (q + (ii << 2)) + 1;
ztest0 = -eml_div(b_eml_xdotc(4 - q, U, qs + 1, U, i), U[qs]);
d_eml_xaxpy(4 - q, ztest0, qs + 1, U, i);
}
for (ii = q; ii + 1 < 5; ii++) {
U[ii + (q << 2)] = -U[ii + (q << 2)];
}
U[qs]++;
for (ii = 1; ii <= q; ii++) {
U[(ii + (q << 2)) - 1] = 0.0F;
}
} else {
for (ii = 0; ii < 4; ii++) {
U[ii + (q << 2)] = 0.0F;
}
U[qs] = 1.0F;
}
}
for (q = 3; q > -1; q += -1) {
if ((q + 1 <= 2) && (e[q] != 0.0F)) {
i = (q + (q << 2)) + 2;
for (ii = q + 1; ii + 1 < 5; ii++) {
qs = (q + (ii << 2)) + 2;
ztest0 = -eml_div(b_eml_xdotc(3 - q, Vf, i, Vf, qs), Vf[i - 1]);
d_eml_xaxpy(3 - q, ztest0, i, Vf, qs);
}
}
for (ii = 0; ii < 4; ii++) {
Vf[ii + (q << 2)] = 0.0F;
}
Vf[q + (q << 2)] = 1.0F;
}
for (q = 0; q < 4; 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 < 4) {
ztest0 = eml_div(e[q], ztest);
}
c_eml_xscal(4, ztest, U, (q << 2) + 1);
}
if ((q + 1 < 4) && (ztest0 != 0.0F)) {
rt = (real32_T)fabs(ztest0);
ztest = eml_div(rt, ztest0);
ztest0 = rt;
s[q + 1] *= ztest;
c_eml_xscal(4, ztest, Vf, ((q + 1) << 2) + 1);
}
e[q] = ztest0;
}
iter = 0;
tiny = eml_div(1.17549435E-38F, 1.1920929E-7F);
snorm = 0.0F;
for (ii = 0; ii < 4; ii++) {
ztest0 = (real32_T)fabs(s[ii]);
ztest = (real32_T)fabs(e[ii]);
if ((ztest0 >= ztest) || rtIsNaNF(ztest)) {
} else {
ztest0 = ztest;
}
if ((snorm >= ztest0) || rtIsNaNF(ztest0)) {
} else {
snorm = ztest0;
}
}
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 (qs = m; qs + 1 >= q + 1; qs--) {
ztest0 = s[qs];
eml_xrotg(&ztest0, &ztest, &rt, &sn);
s[qs] = ztest0;
if (qs + 1 > q + 1) {
ztest = -sn * e[qs - 1];
e[qs - 1] *= rt;
}
b_eml_xrot(Vf, (qs << 2) + 1, ((m + 1) << 2) + 1, rt, sn);
}
break;
case 2:
ztest = e[q - 1];
e[q - 1] = 0.0F;
for (qs = q; qs + 1 <= m + 2; qs++) {
eml_xrotg(&s[qs], &ztest, &rt, &sn);
ztest = -sn * e[qs];
e[qs] *= rt;
b_eml_xrot(U, (qs << 2) + 1, ((q - 1) << 2) + 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])) {
qs = 2;
exitg1 = FALSE;
while ((exitg1 == FALSE) && (qs < 6)) {
i = qs;
if (!rtIsNaNF(varargin_1[qs - 1])) {
sn = varargin_1[qs - 1];
exitg1 = TRUE;
} else {
qs++;
}
}
}
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 (qs = q + 1; qs <= m + 1; qs++) {
eml_xrotg(&ztest, &ztest0, &rt, &sn);
if (qs > q + 1) {
e[qs - 2] = ztest;
}
ztest0 = rt * s[qs - 1];
ztest = sn * e[qs - 1];
e[qs - 1] = rt * e[qs - 1] - sn * s[qs - 1];
b = s[qs];
s[qs] *= rt;
b_eml_xrot(Vf, ((qs - 1) << 2) + 1, (qs << 2) + 1, rt, sn);
s[qs - 1] = ztest0 + ztest;
ztest0 = sn * b;
eml_xrotg(&s[qs - 1], &ztest0, &rt, &sn);
ztest = rt * e[qs - 1] + sn * s[qs];
s[qs] = -sn * e[qs - 1] + rt * s[qs];
ztest0 = sn * e[qs];
e[qs] *= rt;
b_eml_xrot(U, ((qs - 1) << 2) + 1, (qs << 2) + 1, rt, sn);
}
e[m] = ztest;
iter++;
break;
default:
if (s[q] < 0.0F) {
s[q] = -s[q];
c_eml_xscal(4, -1.0F, Vf, (q << 2) + 1);
}
i = q + 1;
while ((q + 1 < 4) && (s[q] < s[i])) {
rt = s[q];
s[q] = s[i];
s[i] = rt;
b_eml_xswap(Vf, (q << 2) + 1, ((q + 1) << 2) + 1);
b_eml_xswap(U, (q << 2) + 1, ((q + 1) << 2) + 1);
q = i;
i++;
}
iter = 0;
m--;
break;
}
}
for (qs = 0; qs < 4; qs++) {
S[qs] = s[qs];
for (i = 0; i < 4; i++) {
V[i + (qs << 2)] = Vf[i + (qs << 2)];
}
}
}
