00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011 #include "rt_nonfinite.h"
00012 #include "Optimal_affine_tracking_3d16_fast_realtime.h"
00013 #include "eig.h"
00014 #include "mldivide.h"
00015 #include "expm.h"
00016 #include "sqrt.h"
00017 #include "Optimal_affine_tracking_3d16_fast_realtime_rtwutil.h"
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 static creal_T b_eml_div(const creal_T x, real_T y);
00029 static void b_eml_matlab_zggev(creal_T A[4], real_T *info, creal_T alpha1[2],
00030 creal_T beta1[2], creal_T V[4]);
00031 static void b_eml_matlab_zhgeqz(creal_T A[4], int32_T ilo, int32_T ihi, creal_T
00032 Z[4], real_T *info, creal_T alpha1[2], creal_T beta1[2]);
00033 static void b_eml_matlab_zlartg(const creal_T f, const creal_T g, real_T *cs,
00034 creal_T *sn);
00035 static void b_eml_matlab_ztgevc(const creal_T A[4], creal_T V[4]);
00036 static int32_T div_s32_floor(int32_T numerator, int32_T denominator);
00037 static void eml_matlab_zggev(creal_T A[9], real_T *info, creal_T alpha1[3],
00038 creal_T beta1[3], creal_T V[9]);
00039 static void eml_matlab_zhgeqz(creal_T A[9], int32_T ilo, int32_T ihi, creal_T Z
00040 [9], real_T *info, creal_T alpha1[3], creal_T beta1[3]);
00041 static void eml_matlab_zlartg(const creal_T f, const creal_T g, real_T *cs,
00042 creal_T *sn, creal_T *r);
00043 static void eml_matlab_ztgevc(const creal_T A[9], creal_T V[9]);
00044
00045
00046 static creal_T b_eml_div(const creal_T x, real_T y)
00047 {
00048 creal_T z;
00049 if (x.im == 0.0) {
00050 z.re = x.re / y;
00051 z.im = 0.0;
00052 } else if (x.re == 0.0) {
00053 z.re = 0.0;
00054 z.im = x.im / y;
00055 } else {
00056 z.re = x.re / y;
00057 z.im = x.im / y;
00058 }
00059
00060 return z;
00061 }
00062
00063 static void b_eml_matlab_zggev(creal_T A[4], real_T *info, creal_T alpha1[2],
00064 creal_T beta1[2], creal_T V[4])
00065 {
00066 real_T anrm;
00067 int32_T nzcount;
00068 boolean_T exitg5;
00069 real_T absxk;
00070 int32_T i;
00071 boolean_T ilascl;
00072 real_T anrmto;
00073 real_T ctoc;
00074 boolean_T notdone;
00075 real_T cfrom1;
00076 real_T cto1;
00077 real_T mul;
00078 creal_T b_A[4];
00079 int8_T rscale[2];
00080 int32_T ilo;
00081 int32_T ihi;
00082 int32_T j;
00083 int32_T ii;
00084 boolean_T exitg3;
00085 int32_T jj;
00086 boolean_T exitg4;
00087 boolean_T guard2 = FALSE;
00088 boolean_T exitg1;
00089 boolean_T exitg2;
00090 boolean_T guard1 = FALSE;
00091 static const int8_T iv8[4] = { 1, 0, 0, 1 };
00092
00093 *info = 0.0;
00094 anrm = 0.0;
00095 nzcount = 0;
00096 exitg5 = FALSE;
00097 while ((exitg5 == 0U) && (nzcount < 4)) {
00098 absxk = rt_hypotd_snf(fabs(A[nzcount].re), fabs(A[nzcount].im));
00099 if (rtIsNaN(absxk)) {
00100 anrm = rtNaN;
00101 exitg5 = TRUE;
00102 } else {
00103 if (absxk > anrm) {
00104 anrm = absxk;
00105 }
00106
00107 nzcount++;
00108 }
00109 }
00110
00111 if (!((!rtIsInf(anrm)) && (!rtIsNaN(anrm)))) {
00112 for (i = 0; i < 2; i++) {
00113 alpha1[i].re = rtNaN;
00114 alpha1[i].im = 0.0;
00115 beta1[i].re = rtNaN;
00116 beta1[i].im = 0.0;
00117 }
00118
00119 for (nzcount = 0; nzcount < 4; nzcount++) {
00120 V[nzcount].re = rtNaN;
00121 V[nzcount].im = 0.0;
00122 }
00123 } else {
00124 ilascl = FALSE;
00125 anrmto = anrm;
00126 if ((anrm > 0.0) && (anrm < 6.7178761075670888E-139)) {
00127 anrmto = 6.7178761075670888E-139;
00128 ilascl = TRUE;
00129 } else {
00130 if (anrm > 1.4885657073574029E+138) {
00131 anrmto = 1.4885657073574029E+138;
00132 ilascl = TRUE;
00133 }
00134 }
00135
00136 if (ilascl) {
00137 absxk = anrm;
00138 ctoc = anrmto;
00139 notdone = TRUE;
00140 while (notdone) {
00141 cfrom1 = absxk * 2.0041683600089728E-292;
00142 cto1 = ctoc / 4.9896007738368E+291;
00143 if ((cfrom1 > ctoc) && (ctoc != 0.0)) {
00144 mul = 2.0041683600089728E-292;
00145 absxk = cfrom1;
00146 } else if (cto1 > absxk) {
00147 mul = 4.9896007738368E+291;
00148 ctoc = cto1;
00149 } else {
00150 mul = ctoc / absxk;
00151 notdone = FALSE;
00152 }
00153
00154 for (nzcount = 0; nzcount < 4; nzcount++) {
00155 A[nzcount].re *= mul;
00156 A[nzcount].im *= mul;
00157 }
00158 }
00159 }
00160
00161 memcpy(&b_A[0], &A[0], sizeof(creal_T) << 2);
00162 for (i = 0; i < 2; i++) {
00163 rscale[i] = 0;
00164 }
00165
00166 ilo = 1;
00167 ihi = 2;
00168 i = 0;
00169 j = 0;
00170 notdone = FALSE;
00171 ii = 2;
00172 exitg3 = FALSE;
00173 while ((exitg3 == 0U) && (ii > 0)) {
00174 nzcount = 0;
00175 i = ii;
00176 j = 2;
00177 jj = 1;
00178 exitg4 = FALSE;
00179 while ((exitg4 == 0U) && (jj <= 2)) {
00180 guard2 = FALSE;
00181 if ((A[(ii + ((jj - 1) << 1)) - 1].re != 0.0) || (A[(ii + ((jj - 1) << 1))
00182 - 1].im != 0.0) || (ii == jj)) {
00183 if (nzcount == 0) {
00184 j = jj;
00185 nzcount = 1;
00186 guard2 = TRUE;
00187 } else {
00188 nzcount = 2;
00189 exitg4 = TRUE;
00190 }
00191 } else {
00192 guard2 = TRUE;
00193 }
00194
00195 if (guard2 == TRUE) {
00196 jj++;
00197 }
00198 }
00199
00200 if (nzcount < 2) {
00201 notdone = TRUE;
00202 exitg3 = TRUE;
00203 } else {
00204 ii--;
00205 }
00206 }
00207
00208 if (!notdone) {
00209 i = 0;
00210 j = 0;
00211 notdone = FALSE;
00212 jj = 1;
00213 exitg1 = FALSE;
00214 while ((exitg1 == 0U) && (jj <= 2)) {
00215 nzcount = 0;
00216 i = 2;
00217 j = jj;
00218 ii = 1;
00219 exitg2 = FALSE;
00220 while ((exitg2 == 0U) && (ii <= 2)) {
00221 guard1 = FALSE;
00222 if ((b_A[(ii + ((jj - 1) << 1)) - 1].re != 0.0) || (b_A[(ii + ((jj - 1)
00223 << 1)) - 1].im != 0.0) || (ii == jj)) {
00224 if (nzcount == 0) {
00225 i = ii;
00226 nzcount = 1;
00227 guard1 = TRUE;
00228 } else {
00229 nzcount = 2;
00230 exitg2 = TRUE;
00231 }
00232 } else {
00233 guard1 = TRUE;
00234 }
00235
00236 if (guard1 == TRUE) {
00237 ii++;
00238 }
00239 }
00240
00241 if (nzcount < 2) {
00242 notdone = TRUE;
00243 exitg1 = TRUE;
00244 } else {
00245 jj++;
00246 }
00247 }
00248
00249 if (!notdone) {
00250 } else {
00251 if (i != 1) {
00252 for (nzcount = 0; nzcount + 1 < 3; nzcount++) {
00253 absxk = b_A[(i + (nzcount << 1)) - 1].re;
00254 ctoc = b_A[(i + (nzcount << 1)) - 1].im;
00255 b_A[(i + (nzcount << 1)) - 1] = b_A[nzcount << 1];
00256 b_A[nzcount << 1].re = absxk;
00257 b_A[nzcount << 1].im = ctoc;
00258 }
00259 }
00260
00261 if (j != 1) {
00262 for (nzcount = 0; nzcount < 2; nzcount++) {
00263 absxk = b_A[nzcount + ((j - 1) << 1)].re;
00264 ctoc = b_A[nzcount + ((j - 1) << 1)].im;
00265 b_A[nzcount + ((j - 1) << 1)] = b_A[nzcount];
00266 b_A[nzcount].re = absxk;
00267 b_A[nzcount].im = ctoc;
00268 }
00269 }
00270
00271 rscale[0] = (int8_T)j;
00272 ilo = 2;
00273 rscale[1] = 2;
00274 }
00275 } else {
00276 if (i != 2) {
00277 for (nzcount = 0; nzcount < 2; nzcount++) {
00278 absxk = b_A[nzcount << 1].re;
00279 ctoc = b_A[nzcount << 1].im;
00280 b_A[nzcount << 1] = b_A[1 + (nzcount << 1)];
00281 b_A[1 + (nzcount << 1)].re = absxk;
00282 b_A[1 + (nzcount << 1)].im = ctoc;
00283 }
00284 }
00285
00286 if (j != 2) {
00287 for (nzcount = 0; nzcount < 2; nzcount++) {
00288 absxk = b_A[nzcount].re;
00289 ctoc = b_A[nzcount].im;
00290 b_A[nzcount] = b_A[2 + nzcount];
00291 b_A[2 + nzcount].re = absxk;
00292 b_A[2 + nzcount].im = ctoc;
00293 }
00294 }
00295
00296 rscale[1] = (int8_T)j;
00297 ihi = 1;
00298 rscale[0] = 1;
00299 }
00300
00301 for (nzcount = 0; nzcount < 4; nzcount++) {
00302 V[nzcount].re = (real_T)iv8[nzcount];
00303 V[nzcount].im = 0.0;
00304 }
00305
00306 b_eml_matlab_zhgeqz(b_A, ilo, ihi, V, info, alpha1, beta1);
00307 if (*info != 0.0) {
00308 } else {
00309 b_eml_matlab_ztgevc(b_A, V);
00310 if ((ilo > 1) && (rscale[0] != 1)) {
00311 for (j = 0; j < 2; j++) {
00312 absxk = V[j << 1].re;
00313 ctoc = V[j << 1].im;
00314 V[j << 1] = V[(rscale[0] + (j << 1)) - 1];
00315 V[(rscale[0] + (j << 1)) - 1].re = absxk;
00316 V[(rscale[0] + (j << 1)) - 1].im = ctoc;
00317 }
00318 }
00319
00320 if ((ihi < 2) && (rscale[1] != 2)) {
00321 for (j = 0; j < 2; j++) {
00322 absxk = V[1 + (j << 1)].re;
00323 ctoc = V[1 + (j << 1)].im;
00324 V[1 + (j << 1)] = V[j << 1];
00325 V[(rscale[1] + (j << 1)) - 1].re = absxk;
00326 V[(rscale[1] + (j << 1)) - 1].im = ctoc;
00327 }
00328 }
00329
00330 for (nzcount = 0; nzcount < 2; nzcount++) {
00331 absxk = fabs(V[1 + (nzcount << 1)].re) + fabs(V[1 + (nzcount << 1)].im);
00332 ctoc = fabs(V[nzcount << 1].re) + fabs(V[nzcount << 1].im);
00333 if (absxk > ctoc) {
00334 ctoc = absxk;
00335 }
00336
00337 if (ctoc >= 6.7178761075670888E-139) {
00338 ctoc = 1.0 / ctoc;
00339 for (ii = 0; ii < 2; ii++) {
00340 V[ii + (nzcount << 1)].re *= ctoc;
00341 V[ii + (nzcount << 1)].im *= ctoc;
00342 }
00343 }
00344 }
00345
00346 if (ilascl) {
00347 notdone = TRUE;
00348 while (notdone) {
00349 cfrom1 = anrmto * 2.0041683600089728E-292;
00350 cto1 = anrm / 4.9896007738368E+291;
00351 if ((cfrom1 > anrm) && (anrm != 0.0)) {
00352 mul = 2.0041683600089728E-292;
00353 anrmto = cfrom1;
00354 } else if (cto1 > anrmto) {
00355 mul = 4.9896007738368E+291;
00356 anrm = cto1;
00357 } else {
00358 mul = anrm / anrmto;
00359 notdone = FALSE;
00360 }
00361
00362 for (nzcount = 0; nzcount < 2; nzcount++) {
00363 alpha1[nzcount].re *= mul;
00364 alpha1[nzcount].im *= mul;
00365 }
00366 }
00367 }
00368 }
00369 }
00370 }
00371
00372 static void b_eml_matlab_zhgeqz(creal_T A[4], int32_T ilo, int32_T ihi, creal_T
00373 Z[4], real_T *info, creal_T alpha1[2], creal_T beta1[2])
00374 {
00375 int32_T i;
00376 real_T eshift_re;
00377 real_T eshift_im;
00378 creal_T ctemp;
00379 real_T rho_re;
00380 real_T rho_im;
00381 real_T anorm;
00382 real_T scale;
00383 real_T sumsq;
00384 boolean_T firstNonZero;
00385 int32_T j;
00386 int32_T istart;
00387 real_T reAij;
00388 real_T imAij;
00389 real_T temp2;
00390 real_T b_atol;
00391 real_T ascale;
00392 boolean_T failed;
00393 boolean_T guard1 = FALSE;
00394 boolean_T guard2 = FALSE;
00395 int32_T ifirst;
00396 int32_T ilast;
00397 int32_T ilastm1;
00398 int32_T iiter;
00399 boolean_T goto60;
00400 boolean_T goto70;
00401 boolean_T goto90;
00402 int32_T jiter;
00403 int32_T exitg1;
00404 boolean_T exitg3;
00405 boolean_T b_guard1 = FALSE;
00406 creal_T d;
00407 creal_T t1;
00408 real_T sigma1_im;
00409 boolean_T exitg2;
00410 *info = 0.0;
00411 for (i = 0; i < 2; i++) {
00412 alpha1[i].re = 0.0;
00413 alpha1[i].im = 0.0;
00414 beta1[i].re = 1.0;
00415 beta1[i].im = 0.0;
00416 }
00417
00418 eshift_re = 0.0;
00419 eshift_im = 0.0;
00420 ctemp.re = 0.0;
00421 ctemp.im = 0.0;
00422 rho_re = 0.0;
00423 rho_im = 0.0;
00424 anorm = 0.0;
00425 if (ilo > ihi) {
00426 } else {
00427 scale = 0.0;
00428 sumsq = 0.0;
00429 firstNonZero = TRUE;
00430 for (j = ilo; j <= ihi; j++) {
00431 istart = j + 1;
00432 if (ihi < istart) {
00433 istart = ihi;
00434 }
00435
00436 for (i = ilo; i <= istart; i++) {
00437 reAij = A[(i + ((j - 1) << 1)) - 1].re;
00438 imAij = A[(i + ((j - 1) << 1)) - 1].im;
00439 if (reAij != 0.0) {
00440 anorm = fabs(reAij);
00441 if (firstNonZero) {
00442 sumsq = 1.0;
00443 scale = anorm;
00444 firstNonZero = FALSE;
00445 } else if (scale < anorm) {
00446 temp2 = scale / anorm;
00447 sumsq = 1.0 + sumsq * temp2 * temp2;
00448 scale = anorm;
00449 } else {
00450 temp2 = anorm / scale;
00451 sumsq += temp2 * temp2;
00452 }
00453 }
00454
00455 if (imAij != 0.0) {
00456 anorm = fabs(imAij);
00457 if (firstNonZero) {
00458 sumsq = 1.0;
00459 scale = anorm;
00460 firstNonZero = FALSE;
00461 } else if (scale < anorm) {
00462 temp2 = scale / anorm;
00463 sumsq = 1.0 + sumsq * temp2 * temp2;
00464 scale = anorm;
00465 } else {
00466 temp2 = anorm / scale;
00467 sumsq += temp2 * temp2;
00468 }
00469 }
00470 }
00471 }
00472
00473 anorm = scale * sqrt(sumsq);
00474 }
00475
00476 reAij = 2.2204460492503131E-16 * anorm;
00477 b_atol = 2.2250738585072014E-308;
00478 if (reAij > 2.2250738585072014E-308) {
00479 b_atol = reAij;
00480 }
00481
00482 reAij = 2.2250738585072014E-308;
00483 if (anorm > 2.2250738585072014E-308) {
00484 reAij = anorm;
00485 }
00486
00487 ascale = 1.0 / reAij;
00488 failed = TRUE;
00489 j = ihi + 1;
00490 while (j < 3) {
00491 alpha1[1] = A[3];
00492 j = 3;
00493 }
00494
00495 guard1 = FALSE;
00496 guard2 = FALSE;
00497 if (ihi >= ilo) {
00498 ifirst = ilo;
00499 istart = ilo;
00500 ilast = ihi - 1;
00501 ilastm1 = ihi - 2;
00502 iiter = 0;
00503 goto60 = FALSE;
00504 goto70 = FALSE;
00505 goto90 = FALSE;
00506 jiter = 1;
00507 do {
00508 exitg1 = 0;
00509 if (jiter <= 30 * ((ihi - ilo) + 1)) {
00510 if (ilast + 1 == ilo) {
00511 goto60 = TRUE;
00512 } else if (fabs(A[ilast + (ilastm1 << 1)].re) + fabs(A[ilast + (ilastm1 <<
00513 1)].im) <= b_atol) {
00514 A[ilast + (ilastm1 << 1)].re = 0.0;
00515 A[ilast + (ilastm1 << 1)].im = 0.0;
00516 goto60 = TRUE;
00517 } else {
00518 j = ilastm1 + 1;
00519 exitg3 = FALSE;
00520 while ((exitg3 == 0U) && (j >= ilo)) {
00521 if (j == ilo) {
00522 firstNonZero = TRUE;
00523 } else if (fabs(A[j - 1].re) + fabs(A[j - 1].im) <= b_atol) {
00524 A[j - 1].re = 0.0;
00525 A[j - 1].im = 0.0;
00526 firstNonZero = TRUE;
00527 } else {
00528 firstNonZero = FALSE;
00529 }
00530
00531 if (firstNonZero) {
00532 ifirst = j;
00533 goto70 = TRUE;
00534 exitg3 = TRUE;
00535 } else {
00536 j--;
00537 }
00538 }
00539 }
00540
00541 if (goto60 || goto70) {
00542 firstNonZero = TRUE;
00543 } else {
00544 firstNonZero = FALSE;
00545 }
00546
00547 if (!firstNonZero) {
00548 for (i = 0; i < 2; i++) {
00549 alpha1[i].re = rtNaN;
00550 alpha1[i].im = 0.0;
00551 beta1[i].re = rtNaN;
00552 beta1[i].im = 0.0;
00553 }
00554
00555 for (istart = 0; istart < 2; istart++) {
00556 for (ifirst = 0; ifirst < 2; ifirst++) {
00557 Z[ifirst + (istart << 1)].re = rtNaN;
00558 Z[ifirst + (istart << 1)].im = 0.0;
00559 }
00560 }
00561
00562 *info = -1.0;
00563 exitg1 = 1;
00564 } else {
00565 b_guard1 = FALSE;
00566 if (goto60) {
00567 goto60 = FALSE;
00568 alpha1[ilast] = A[ilast + (ilast << 1)];
00569 ilast = ilastm1;
00570 ilastm1--;
00571 if (ilast + 1 < ilo) {
00572 failed = FALSE;
00573 guard2 = TRUE;
00574 exitg1 = 1;
00575 } else {
00576 iiter = 0;
00577 eshift_re = 0.0;
00578 eshift_im = 0.0;
00579 b_guard1 = TRUE;
00580 }
00581 } else {
00582 if (goto70) {
00583 goto70 = FALSE;
00584 iiter++;
00585 if (iiter - div_s32_floor(iiter, 10) * 10 != 0) {
00586 d.re = -(A[ilast + (ilast << 1)].re - A[ilastm1 + (ilastm1 << 1)]
00587 .re);
00588 d.im = -(A[ilast + (ilast << 1)].im - A[ilastm1 + (ilastm1 << 1)]
00589 .im);
00590 t1 = b_eml_div(d, 2.0);
00591 anorm = A[ilastm1 + (ilast << 1)].re * A[ilast + (ilastm1 << 1)]
00592 .re - A[ilastm1 + (ilast << 1)].im * A[ilast + (ilastm1 << 1)]
00593 .im;
00594 reAij = A[ilastm1 + (ilast << 1)].re * A[ilast + (ilastm1 << 1)]
00595 .im + A[ilastm1 + (ilast << 1)].im * A[ilast + (ilastm1 << 1)]
00596 .re;
00597 d.re = (t1.re * t1.re - t1.im * t1.im) + anorm;
00598 d.im = (t1.re * t1.im + t1.im * t1.re) + reAij;
00599 b_sqrt(&d);
00600 temp2 = A[ilastm1 + (ilastm1 << 1)].re - (t1.re - d.re);
00601 sigma1_im = A[ilastm1 + (ilastm1 << 1)].im - (t1.im - d.im);
00602 scale = A[ilastm1 + (ilastm1 << 1)].re - (t1.re + d.re);
00603 sumsq = A[ilastm1 + (ilastm1 << 1)].im - (t1.im + d.im);
00604 rho_re = temp2 - A[ilast + (ilast << 1)].re;
00605 rho_im = sigma1_im - A[ilast + (ilast << 1)].im;
00606 anorm = scale - A[ilast + (ilast << 1)].re;
00607 reAij = sumsq - A[ilast + (ilast << 1)].im;
00608 if (rt_hypotd_snf(fabs(rho_re), fabs(rho_im)) <= rt_hypotd_snf
00609 (fabs(anorm), fabs(reAij))) {
00610 scale = temp2;
00611 sumsq = sigma1_im;
00612 rho_re = t1.re - d.re;
00613 rho_im = t1.im - d.im;
00614 } else {
00615 rho_re = t1.re + d.re;
00616 rho_im = t1.im + d.im;
00617 }
00618 } else {
00619 eshift_re += A[ilast + (ilastm1 << 1)].re;
00620 eshift_im += A[ilast + (ilastm1 << 1)].im;
00621 scale = eshift_re;
00622 sumsq = eshift_im;
00623 }
00624
00625 j = ilastm1;
00626 exitg2 = FALSE;
00627 while ((exitg2 == 0U) && (j + 1 > ifirst)) {
00628 istart = 2;
00629 ctemp.re = A[3].re - scale;
00630 ctemp.im = A[3].im - sumsq;
00631 anorm = ascale * (fabs(ctemp.re) + fabs(ctemp.im));
00632 temp2 = ascale * (fabs(A[3].re) + fabs(A[3].im));
00633 reAij = anorm;
00634 if (temp2 > anorm) {
00635 reAij = temp2;
00636 }
00637
00638 if ((reAij < 1.0) && (reAij != 0.0)) {
00639 anorm /= reAij;
00640 temp2 /= reAij;
00641 }
00642
00643 if ((fabs(A[1].re) + fabs(A[1].im)) * temp2 <= anorm * b_atol) {
00644 goto90 = TRUE;
00645 exitg2 = TRUE;
00646 } else {
00647 j = 0;
00648 }
00649 }
00650
00651 if (!goto90) {
00652 istart = ifirst;
00653 if (ifirst == ilastm1 + 1) {
00654 ctemp.re = rho_re;
00655 ctemp.im = rho_im;
00656 } else {
00657 ctemp.re = A[(ifirst + ((ifirst - 1) << 1)) - 1].re - scale;
00658 ctemp.im = A[(ifirst + ((ifirst - 1) << 1)) - 1].im - sumsq;
00659 }
00660
00661 goto90 = TRUE;
00662 }
00663 }
00664
00665 if (goto90) {
00666 goto90 = FALSE;
00667 d = A[1 + ((istart - 1) << 1)];
00668 b_eml_matlab_zlartg(ctemp, d, &imAij, &t1);
00669 j = istart;
00670 while (j < ilast + 1) {
00671 for (j = 0; j < 2; j++) {
00672 d.re = imAij * A[j << 1].re;
00673 d.im = imAij * A[j << 1].im;
00674 temp2 = t1.re * A[1 + (j << 1)].re - t1.im * A[1 + (j << 1)].
00675 im;
00676 sigma1_im = t1.re * A[1 + (j << 1)].im + t1.im * A[1 + (j << 1)]
00677 .re;
00678 anorm = A[j << 1].re;
00679 reAij = A[j << 1].im;
00680 scale = A[j << 1].im;
00681 sumsq = A[j << 1].re;
00682 A[1 + (j << 1)].re = imAij * A[1 + (j << 1)].re - (t1.re *
00683 anorm + t1.im * reAij);
00684 A[1 + (j << 1)].im = imAij * A[1 + (j << 1)].im - (t1.re *
00685 scale - t1.im * sumsq);
00686 A[j << 1].re = d.re + temp2;
00687 A[j << 1].im = d.im + sigma1_im;
00688 }
00689
00690 t1.re = -t1.re;
00691 t1.im = -t1.im;
00692 for (i = 0; i < 2; i++) {
00693 d.re = imAij * A[2 + i].re;
00694 d.im = imAij * A[2 + i].im;
00695 temp2 = t1.re * A[i].re - t1.im * A[i].im;
00696 sigma1_im = t1.re * A[i].im + t1.im * A[i].re;
00697 anorm = A[2 + i].re;
00698 reAij = A[2 + i].im;
00699 scale = A[2 + i].im;
00700 sumsq = A[2 + i].re;
00701 A[i].re = imAij * A[i].re - (t1.re * anorm + t1.im * reAij);
00702 A[i].im = imAij * A[i].im - (t1.re * scale - t1.im * sumsq);
00703 A[2 + i].re = d.re + temp2;
00704 A[2 + i].im = d.im + sigma1_im;
00705 }
00706
00707 for (i = 0; i < 2; i++) {
00708 d.re = imAij * Z[2 + i].re;
00709 d.im = imAij * Z[2 + i].im;
00710 temp2 = t1.re * Z[i].re - t1.im * Z[i].im;
00711 sigma1_im = t1.re * Z[i].im + t1.im * Z[i].re;
00712 anorm = Z[2 + i].re;
00713 reAij = Z[2 + i].im;
00714 scale = Z[2 + i].im;
00715 sumsq = Z[2 + i].re;
00716 Z[i].re = imAij * Z[i].re - (t1.re * anorm + t1.im * reAij);
00717 Z[i].im = imAij * Z[i].im - (t1.re * scale - t1.im * sumsq);
00718 Z[2 + i].re = d.re + temp2;
00719 Z[2 + i].im = d.im + sigma1_im;
00720 }
00721
00722 j = 2;
00723 }
00724 }
00725
00726 b_guard1 = TRUE;
00727 }
00728
00729 if (b_guard1 == TRUE) {
00730 jiter++;
00731 }
00732 }
00733 } else {
00734 guard2 = TRUE;
00735 exitg1 = 1;
00736 }
00737 } while (exitg1 == 0U);
00738 } else {
00739 guard1 = TRUE;
00740 }
00741
00742 if (guard2 == TRUE) {
00743 if (failed) {
00744 *info = (real_T)(ilast + 1);
00745 for (istart = 0; istart + 1 <= ilast + 1; istart++) {
00746 alpha1[istart].re = rtNaN;
00747 alpha1[istart].im = 0.0;
00748 beta1[istart].re = rtNaN;
00749 beta1[istart].im = 0.0;
00750 }
00751
00752 for (istart = 0; istart < 2; istart++) {
00753 for (ifirst = 0; ifirst < 2; ifirst++) {
00754 Z[ifirst + (istart << 1)].re = rtNaN;
00755 Z[ifirst + (istart << 1)].im = 0.0;
00756 }
00757 }
00758 } else {
00759 guard1 = TRUE;
00760 }
00761 }
00762
00763 if (guard1 == TRUE) {
00764 for (j = 0; j + 1 <= ilo - 1; j++) {
00765 alpha1[j] = A[j + (j << 1)];
00766 }
00767 }
00768 }
00769
00770 static void b_eml_matlab_zlartg(const creal_T f, const creal_T g, real_T *cs,
00771 creal_T *sn)
00772 {
00773 real_T scale;
00774 real_T f2s;
00775 real_T g2;
00776 real_T fs_re;
00777 real_T fs_im;
00778 real_T gs_re;
00779 real_T gs_im;
00780 boolean_T guard1 = FALSE;
00781 real_T g2s;
00782 scale = fabs(f.re);
00783 f2s = fabs(f.im);
00784 if (f2s > scale) {
00785 scale = f2s;
00786 }
00787
00788 f2s = fabs(g.re);
00789 g2 = fabs(g.im);
00790 if (g2 > f2s) {
00791 f2s = g2;
00792 }
00793
00794 if (f2s > scale) {
00795 scale = f2s;
00796 }
00797
00798 fs_re = f.re;
00799 fs_im = f.im;
00800 gs_re = g.re;
00801 gs_im = g.im;
00802 guard1 = FALSE;
00803 if (scale >= 7.4428285367870146E+137) {
00804 do {
00805 fs_re *= 1.3435752215134178E-138;
00806 fs_im *= 1.3435752215134178E-138;
00807 gs_re *= 1.3435752215134178E-138;
00808 gs_im *= 1.3435752215134178E-138;
00809 scale *= 1.3435752215134178E-138;
00810 } while (!(scale < 7.4428285367870146E+137));
00811
00812 guard1 = TRUE;
00813 } else if (scale <= 1.3435752215134178E-138) {
00814 if ((g.re == 0.0) && (g.im == 0.0)) {
00815 *cs = 1.0;
00816 sn->re = 0.0;
00817 sn->im = 0.0;
00818 } else {
00819 do {
00820 fs_re *= 7.4428285367870146E+137;
00821 fs_im *= 7.4428285367870146E+137;
00822 gs_re *= 7.4428285367870146E+137;
00823 gs_im *= 7.4428285367870146E+137;
00824 scale *= 7.4428285367870146E+137;
00825 } while (!(scale > 1.3435752215134178E-138));
00826
00827 guard1 = TRUE;
00828 }
00829 } else {
00830 guard1 = TRUE;
00831 }
00832
00833 if (guard1 == TRUE) {
00834 scale = fs_re * fs_re + fs_im * fs_im;
00835 g2 = gs_re * gs_re + gs_im * gs_im;
00836 f2s = g2;
00837 if (1.0 > g2) {
00838 f2s = 1.0;
00839 }
00840
00841 if (scale <= f2s * 2.0041683600089728E-292) {
00842 if ((f.re == 0.0) && (f.im == 0.0)) {
00843 *cs = 0.0;
00844 scale = rt_hypotd_snf(fabs(gs_re), fabs(gs_im));
00845 sn->re = gs_re / scale;
00846 sn->im = -gs_im / scale;
00847 } else {
00848 g2s = sqrt(g2);
00849 *cs = rt_hypotd_snf(fabs(fs_re), fabs(fs_im)) / g2s;
00850 f2s = fabs(f.re);
00851 g2 = fabs(f.im);
00852 if (g2 > f2s) {
00853 f2s = g2;
00854 }
00855
00856 if (f2s > 1.0) {
00857 scale = rt_hypotd_snf(fabs(f.re), fabs(f.im));
00858 fs_re = f.re / scale;
00859 fs_im = f.im / scale;
00860 } else {
00861 f2s = 7.4428285367870146E+137 * f.re;
00862 g2 = 7.4428285367870146E+137 * f.im;
00863 scale = rt_hypotd_snf(fabs(f2s), fabs(g2));
00864 fs_re = f2s / scale;
00865 fs_im = g2 / scale;
00866 }
00867
00868 gs_re /= g2s;
00869 gs_im = -gs_im / g2s;
00870 sn->re = fs_re * gs_re - fs_im * gs_im;
00871 sn->im = fs_re * gs_im + fs_im * gs_re;
00872 }
00873 } else {
00874 f2s = sqrt(1.0 + g2 / scale);
00875 *cs = 1.0 / f2s;
00876 scale += g2;
00877 fs_re = f2s * fs_re / scale;
00878 fs_im = f2s * fs_im / scale;
00879 sn->re = fs_re * gs_re - fs_im * -gs_im;
00880 sn->im = fs_re * -gs_im + fs_im * gs_re;
00881 }
00882 }
00883 }
00884
00885 static void b_eml_matlab_ztgevc(const creal_T A[4], creal_T V[4])
00886 {
00887 creal_T work1[2];
00888 creal_T work2[2];
00889 real_T rworka[2];
00890 int32_T i;
00891 real_T y;
00892 real_T anorm;
00893 real_T dmin;
00894 real_T ascale;
00895 int32_T je;
00896 real_T temp;
00897 real_T salpha_re;
00898 real_T salpha_im;
00899 real_T acoeff;
00900 boolean_T b2;
00901 boolean_T b3;
00902 real_T scale;
00903 real_T b_y;
00904 creal_T d;
00905 creal_T b_work1;
00906 int32_T jc;
00907 for (i = 0; i < 2; i++) {
00908 work1[i].re = 0.0;
00909 work1[i].im = 0.0;
00910 work2[i].re = 0.0;
00911 work2[i].im = 0.0;
00912 rworka[i] = 0.0;
00913 }
00914
00915 rworka[1] = fabs(A[2].re) + fabs(A[2].im);
00916 y = rworka[1] + (fabs(A[3].re) + fabs(A[3].im));
00917 anorm = fabs(A[0].re) + fabs(A[0].im);
00918 if (y > anorm) {
00919 anorm = y;
00920 }
00921
00922 dmin = anorm;
00923 if (2.2250738585072014E-308 > anorm) {
00924 dmin = 2.2250738585072014E-308;
00925 }
00926
00927 ascale = 1.0 / dmin;
00928 for (je = 0; je < 2; je++) {
00929 y = (fabs(A[(((1 - je) << 1) - je) + 1].re) + fabs(A[(((1 - je) << 1) - je)
00930 + 1].im)) * ascale;
00931 if (1.0 > y) {
00932 y = 1.0;
00933 }
00934
00935 temp = 1.0 / y;
00936 salpha_re = ascale * (temp * A[(((1 - je) << 1) - je) + 1].re);
00937 salpha_im = ascale * (temp * A[(((1 - je) << 1) - je) + 1].im);
00938 acoeff = temp * ascale;
00939 if ((fabs(temp) >= 2.2250738585072014E-308) && (fabs(acoeff) <
00940 2.0041683600089728E-292)) {
00941 b2 = TRUE;
00942 } else {
00943 b2 = FALSE;
00944 }
00945
00946 if ((fabs(salpha_re) + fabs(salpha_im) >= 2.2250738585072014E-308) && (fabs
00947 (salpha_re) + fabs(salpha_im) < 2.0041683600089728E-292)) {
00948 b3 = TRUE;
00949 } else {
00950 b3 = FALSE;
00951 }
00952
00953 scale = 1.0;
00954 if (b2) {
00955 dmin = anorm;
00956 if (4.9896007738368E+291 < anorm) {
00957 dmin = 4.9896007738368E+291;
00958 }
00959
00960 scale = 2.0041683600089728E-292 / fabs(temp) * dmin;
00961 }
00962
00963 if (b3) {
00964 dmin = 2.0041683600089728E-292 / (fabs(salpha_re) + fabs(salpha_im));
00965 if (dmin > scale) {
00966 scale = dmin;
00967 }
00968 }
00969
00970 if (b2 || b3) {
00971 y = fabs(acoeff);
00972 b_y = fabs(salpha_re) + fabs(salpha_im);
00973 if (1.0 > y) {
00974 y = 1.0;
00975 }
00976
00977 if (b_y > y) {
00978 y = b_y;
00979 }
00980
00981 y = 1.0 / (2.2250738585072014E-308 * y);
00982 if (y < scale) {
00983 scale = y;
00984 }
00985
00986 if (b2) {
00987 acoeff = ascale * (scale * temp);
00988 } else {
00989 acoeff *= scale;
00990 }
00991
00992 if (b3) {
00993 salpha_re *= scale;
00994 salpha_im *= scale;
00995 } else {
00996 salpha_re *= scale;
00997 salpha_im *= scale;
00998 }
00999 }
01000
01001 for (i = 0; i < 2; i++) {
01002 work1[i].re = 0.0;
01003 work1[i].im = 0.0;
01004 }
01005
01006 work1[1 - je].re = 1.0;
01007 work1[1 - je].im = 0.0;
01008 dmin = 2.2204460492503131E-16 * fabs(acoeff) * anorm;
01009 y = 2.2204460492503131E-16 * (fabs(salpha_re) + fabs(salpha_im));
01010 if (y > dmin) {
01011 dmin = y;
01012 }
01013
01014 if (2.2250738585072014E-308 > dmin) {
01015 dmin = 2.2250738585072014E-308;
01016 }
01017
01018 i = 0;
01019 while (i <= -je) {
01020 work1[0].re = acoeff * A[(1 - je) << 1].re;
01021 work1[0].im = acoeff * A[(1 - je) << 1].im;
01022 i = 1;
01023 }
01024
01025 work1[1 - je].re = 1.0;
01026 work1[1 - je].im = 0.0;
01027 i = 0;
01028 while (i <= -je) {
01029 d.re = acoeff * A[0].re - salpha_re;
01030 d.im = acoeff * A[0].im - salpha_im;
01031 if (fabs(d.re) + fabs(d.im) <= dmin) {
01032 d.re = dmin;
01033 d.im = 0.0;
01034 }
01035
01036 if ((fabs(d.re) + fabs(d.im) < 1.0) && (fabs(work1[0].re) + fabs(work1[0].
01037 im) >= 2.2471164185778949E+307 * (fabs(d.re) + fabs(d.im)))) {
01038 temp = 1.0 / (fabs(work1[0].re) + fabs(work1[0].im));
01039 for (i = 0; i <= 1 - je; i++) {
01040 work1[i].re *= temp;
01041 work1[i].im *= temp;
01042 }
01043 }
01044
01045 b_work1.re = -work1[0].re;
01046 b_work1.im = -work1[0].im;
01047 work1[0] = c_eml_div(b_work1, d);
01048 i = 1;
01049 }
01050
01051 for (i = 0; i < 2; i++) {
01052 work2[i].re = 0.0;
01053 work2[i].im = 0.0;
01054 }
01055
01056 for (jc = 0; jc <= 1 - je; jc++) {
01057 for (i = 0; i < 2; i++) {
01058 dmin = V[i + (jc << 1)].re * work1[jc].re - V[i + (jc << 1)].im *
01059 work1[jc].im;
01060 b_y = V[i + (jc << 1)].re * work1[jc].im + V[i + (jc << 1)].im *
01061 work1[jc].re;
01062 work2[i].re += dmin;
01063 work2[i].im += b_y;
01064 }
01065 }
01066
01067 y = fabs(work2[1].re) + fabs(work2[1].im);
01068 dmin = fabs(work2[0].re) + fabs(work2[0].im);
01069 if (y > dmin) {
01070 dmin = y;
01071 }
01072
01073 if (dmin > 2.2250738585072014E-308) {
01074 temp = 1.0 / dmin;
01075 for (i = 0; i < 2; i++) {
01076 V[i + ((1 - je) << 1)].re = temp * work2[i].re;
01077 V[i + ((1 - je) << 1)].im = temp * work2[i].im;
01078 }
01079 } else {
01080 for (i = 0; i < 2; i++) {
01081 V[i + ((1 - je) << 1)].re = 0.0;
01082 V[i + ((1 - je) << 1)].im = 0.0;
01083 }
01084 }
01085 }
01086 }
01087
01088 static int32_T div_s32_floor(int32_T numerator, int32_T denominator)
01089 {
01090 int32_T quotient;
01091 uint32_T absNumerator;
01092 uint32_T absDenominator;
01093 int32_T quotientNeedsNegation;
01094 uint32_T tempAbsQuotient;
01095 if (denominator == 0) {
01096 quotient = numerator >= 0 ? MAX_int32_T : MIN_int32_T;
01097 } else {
01098 absNumerator = (uint32_T)(numerator >= 0 ? numerator : -numerator);
01099 absDenominator = (uint32_T)(denominator >= 0 ? denominator : -denominator);
01100 quotientNeedsNegation = (int32_T)((int32_T)(numerator < 0) != (int32_T)
01101 (denominator < 0));
01102 tempAbsQuotient = absNumerator / absDenominator;
01103 if ((uint32_T)quotientNeedsNegation) {
01104 absNumerator %= absDenominator;
01105 if (absNumerator > (uint32_T)0) {
01106 tempAbsQuotient++;
01107 }
01108 }
01109
01110 quotient = (uint32_T)quotientNeedsNegation ? -(int32_T)tempAbsQuotient :
01111 (int32_T)tempAbsQuotient;
01112 }
01113
01114 return quotient;
01115 }
01116
01117 static void eml_matlab_zggev(creal_T A[9], real_T *info, creal_T alpha1[3],
01118 creal_T beta1[3], creal_T V[9])
01119 {
01120 real_T anrm;
01121 int32_T nzcount;
01122 boolean_T exitg7;
01123 real_T absxk;
01124 int32_T i;
01125 int32_T ii;
01126 boolean_T ilascl;
01127 real_T anrmto;
01128 real_T ctoc;
01129 boolean_T notdone;
01130 real_T cfrom1;
01131 real_T cto1;
01132 real_T mul;
01133 creal_T b_A[9];
01134 int32_T rscale[3];
01135 int32_T ilo;
01136 int32_T ihi;
01137 int32_T exitg2;
01138 int32_T j;
01139 boolean_T exitg5;
01140 int32_T jj;
01141 boolean_T exitg6;
01142 boolean_T guard2 = FALSE;
01143 creal_T atmp;
01144 int32_T exitg1;
01145 boolean_T exitg3;
01146 boolean_T exitg4;
01147 boolean_T guard1 = FALSE;
01148 static const int8_T iv4[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
01149
01150 creal_T s;
01151 *info = 0.0;
01152 anrm = 0.0;
01153 nzcount = 0;
01154 exitg7 = FALSE;
01155 while ((exitg7 == 0U) && (nzcount < 9)) {
01156 absxk = rt_hypotd_snf(fabs(A[nzcount].re), fabs(A[nzcount].im));
01157 if (rtIsNaN(absxk)) {
01158 anrm = rtNaN;
01159 exitg7 = TRUE;
01160 } else {
01161 if (absxk > anrm) {
01162 anrm = absxk;
01163 }
01164
01165 nzcount++;
01166 }
01167 }
01168
01169 if (!((!rtIsInf(anrm)) && (!rtIsNaN(anrm)))) {
01170 for (i = 0; i < 3; i++) {
01171 alpha1[i].re = rtNaN;
01172 alpha1[i].im = 0.0;
01173 beta1[i].re = rtNaN;
01174 beta1[i].im = 0.0;
01175 }
01176
01177 for (ii = 0; ii < 9; ii++) {
01178 V[ii].re = rtNaN;
01179 V[ii].im = 0.0;
01180 }
01181 } else {
01182 ilascl = FALSE;
01183 anrmto = anrm;
01184 if ((anrm > 0.0) && (anrm < 6.7178761075670888E-139)) {
01185 anrmto = 6.7178761075670888E-139;
01186 ilascl = TRUE;
01187 } else {
01188 if (anrm > 1.4885657073574029E+138) {
01189 anrmto = 1.4885657073574029E+138;
01190 ilascl = TRUE;
01191 }
01192 }
01193
01194 if (ilascl) {
01195 absxk = anrm;
01196 ctoc = anrmto;
01197 notdone = TRUE;
01198 while (notdone) {
01199 cfrom1 = absxk * 2.0041683600089728E-292;
01200 cto1 = ctoc / 4.9896007738368E+291;
01201 if ((cfrom1 > ctoc) && (ctoc != 0.0)) {
01202 mul = 2.0041683600089728E-292;
01203 absxk = cfrom1;
01204 } else if (cto1 > absxk) {
01205 mul = 4.9896007738368E+291;
01206 ctoc = cto1;
01207 } else {
01208 mul = ctoc / absxk;
01209 notdone = FALSE;
01210 }
01211
01212 for (ii = 0; ii < 9; ii++) {
01213 A[ii].re *= mul;
01214 A[ii].im *= mul;
01215 }
01216 }
01217 }
01218
01219 memcpy(&b_A[0], &A[0], 9U * sizeof(creal_T));
01220 for (i = 0; i < 3; i++) {
01221 rscale[i] = 0;
01222 }
01223
01224 ilo = 1;
01225 ihi = 3;
01226 do {
01227 exitg2 = 0;
01228 i = 0;
01229 j = 0;
01230 notdone = FALSE;
01231 ii = ihi;
01232 exitg5 = FALSE;
01233 while ((exitg5 == 0U) && (ii > 0)) {
01234 nzcount = 0;
01235 i = ii;
01236 j = ihi;
01237 jj = 1;
01238 exitg6 = FALSE;
01239 while ((exitg6 == 0U) && (jj <= ihi)) {
01240 guard2 = FALSE;
01241 if ((b_A[(ii + 3 * (jj - 1)) - 1].re != 0.0) || (b_A[(ii + 3 * (jj - 1))
01242 - 1].im != 0.0) || (ii == jj)) {
01243 if (nzcount == 0) {
01244 j = jj;
01245 nzcount = 1;
01246 guard2 = TRUE;
01247 } else {
01248 nzcount = 2;
01249 exitg6 = TRUE;
01250 }
01251 } else {
01252 guard2 = TRUE;
01253 }
01254
01255 if (guard2 == TRUE) {
01256 jj++;
01257 }
01258 }
01259
01260 if (nzcount < 2) {
01261 notdone = TRUE;
01262 exitg5 = TRUE;
01263 } else {
01264 ii--;
01265 }
01266 }
01267
01268 if (!notdone) {
01269 exitg2 = 2;
01270 } else {
01271 if (i != ihi) {
01272 for (nzcount = 0; nzcount < 3; nzcount++) {
01273 atmp = b_A[(i + 3 * nzcount) - 1];
01274 b_A[(i + 3 * nzcount) - 1] = b_A[(ihi + 3 * nzcount) - 1];
01275 b_A[(ihi + 3 * nzcount) - 1] = atmp;
01276 }
01277 }
01278
01279 if (j != ihi) {
01280 for (nzcount = 0; nzcount + 1 <= ihi; nzcount++) {
01281 atmp = b_A[nzcount + 3 * (j - 1)];
01282 b_A[nzcount + 3 * (j - 1)] = b_A[nzcount + 3 * (ihi - 1)];
01283 b_A[nzcount + 3 * (ihi - 1)] = atmp;
01284 }
01285 }
01286
01287 rscale[ihi - 1] = j;
01288 ihi--;
01289 if (ihi == 1) {
01290 rscale[0] = 1;
01291 exitg2 = 1;
01292 }
01293 }
01294 } while (exitg2 == 0U);
01295
01296 if (exitg2 == 1U) {
01297 } else {
01298 do {
01299 exitg1 = 0;
01300 i = 0;
01301 j = 0;
01302 notdone = FALSE;
01303 jj = ilo;
01304 exitg3 = FALSE;
01305 while ((exitg3 == 0U) && (jj <= ihi)) {
01306 nzcount = 0;
01307 i = ihi;
01308 j = jj;
01309 ii = ilo;
01310 exitg4 = FALSE;
01311 while ((exitg4 == 0U) && (ii <= ihi)) {
01312 guard1 = FALSE;
01313 if ((b_A[(ii + 3 * (jj - 1)) - 1].re != 0.0) || (b_A[(ii + 3 * (jj -
01314 1)) - 1].im != 0.0) || (ii == jj)) {
01315 if (nzcount == 0) {
01316 i = ii;
01317 nzcount = 1;
01318 guard1 = TRUE;
01319 } else {
01320 nzcount = 2;
01321 exitg4 = TRUE;
01322 }
01323 } else {
01324 guard1 = TRUE;
01325 }
01326
01327 if (guard1 == TRUE) {
01328 ii++;
01329 }
01330 }
01331
01332 if (nzcount < 2) {
01333 notdone = TRUE;
01334 exitg3 = TRUE;
01335 } else {
01336 jj++;
01337 }
01338 }
01339
01340 if (!notdone) {
01341 exitg1 = 1;
01342 } else {
01343 if (i != ilo) {
01344 for (nzcount = ilo - 1; nzcount + 1 < 4; nzcount++) {
01345 atmp = b_A[(i + 3 * nzcount) - 1];
01346 b_A[(i + 3 * nzcount) - 1] = b_A[(ilo + 3 * nzcount) - 1];
01347 b_A[(ilo + 3 * nzcount) - 1] = atmp;
01348 }
01349 }
01350
01351 if (j != ilo) {
01352 for (nzcount = 0; nzcount + 1 <= ihi; nzcount++) {
01353 atmp = b_A[nzcount + 3 * (j - 1)];
01354 b_A[nzcount + 3 * (j - 1)] = b_A[nzcount + 3 * (ilo - 1)];
01355 b_A[nzcount + 3 * (ilo - 1)] = atmp;
01356 }
01357 }
01358
01359 rscale[ilo - 1] = j;
01360 ilo++;
01361 if (ilo == ihi) {
01362 rscale[ilo - 1] = ilo;
01363 exitg1 = 1;
01364 }
01365 }
01366 } while (exitg1 == 0U);
01367 }
01368
01369 for (ii = 0; ii < 9; ii++) {
01370 V[ii].re = (real_T)iv4[ii];
01371 V[ii].im = 0.0;
01372 }
01373
01374 if (ihi < ilo + 2) {
01375 } else {
01376 ii = ilo;
01377 while (ii < 2) {
01378 eml_matlab_zlartg(b_A[1], b_A[2], &cfrom1, &s, &atmp);
01379 b_A[1] = atmp;
01380 b_A[2].re = 0.0;
01381 b_A[2].im = 0.0;
01382 for (j = 0; j < 2; j++) {
01383 atmp.re = cfrom1 * b_A[1 + 3 * (j + 1)].re;
01384 atmp.im = cfrom1 * b_A[1 + 3 * (j + 1)].im;
01385 cto1 = s.re * b_A[2 + 3 * (j + 1)].re - s.im * b_A[2 + 3 * (j + 1)].im;
01386 mul = s.re * b_A[2 + 3 * (j + 1)].im + s.im * b_A[2 + 3 * (j + 1)].re;
01387 absxk = b_A[1 + 3 * (j + 1)].im;
01388 ctoc = b_A[1 + 3 * (j + 1)].re;
01389 b_A[2 + 3 * (j + 1)].re = cfrom1 * b_A[2 + 3 * (j + 1)].re - (s.re *
01390 b_A[1 + 3 * (j + 1)].re + s.im * b_A[1 + 3 * (j + 1)].im);
01391 b_A[2 + 3 * (j + 1)].im = cfrom1 * b_A[2 + 3 * (j + 1)].im - (s.re *
01392 absxk - s.im * ctoc);
01393 b_A[1 + 3 * (j + 1)].re = atmp.re + cto1;
01394 b_A[1 + 3 * (j + 1)].im = atmp.im + mul;
01395 }
01396
01397 s.re = -s.re;
01398 s.im = -s.im;
01399 for (i = ilo - 1; i + 1 < 4; i++) {
01400 atmp.re = cfrom1 * b_A[6 + i].re;
01401 atmp.im = cfrom1 * b_A[6 + i].im;
01402 cto1 = s.re * b_A[3 + i].re - s.im * b_A[3 + i].im;
01403 mul = s.re * b_A[3 + i].im + s.im * b_A[3 + i].re;
01404 absxk = b_A[6 + i].im;
01405 ctoc = b_A[6 + i].re;
01406 b_A[3 + i].re = cfrom1 * b_A[3 + i].re - (s.re * b_A[6 + i].re + s.im *
01407 b_A[6 + i].im);
01408 b_A[3 + i].im = cfrom1 * b_A[3 + i].im - (s.re * absxk - s.im * ctoc);
01409 b_A[6 + i].re = atmp.re + cto1;
01410 b_A[6 + i].im = atmp.im + mul;
01411 }
01412
01413 for (i = 0; i < 3; i++) {
01414 atmp.re = cfrom1 * V[6 + i].re;
01415 atmp.im = cfrom1 * V[6 + i].im;
01416 cto1 = s.re * V[3 + i].re - s.im * V[3 + i].im;
01417 mul = s.re * V[3 + i].im + s.im * V[3 + i].re;
01418 absxk = V[6 + i].im;
01419 ctoc = V[6 + i].re;
01420 V[3 + i].re = cfrom1 * V[3 + i].re - (s.re * V[6 + i].re + s.im * V[6
01421 + i].im);
01422 V[3 + i].im = cfrom1 * V[3 + i].im - (s.re * absxk - s.im * ctoc);
01423 V[6 + i].re = atmp.re + cto1;
01424 V[6 + i].im = atmp.im + mul;
01425 }
01426
01427 ii = 2;
01428 }
01429 }
01430
01431 eml_matlab_zhgeqz(b_A, ilo, ihi, V, info, alpha1, beta1);
01432 if (*info != 0.0) {
01433 } else {
01434 eml_matlab_ztgevc(b_A, V);
01435 if (ilo > 1) {
01436 for (i = ilo - 2; i + 1 >= 1; i--) {
01437 if (rscale[i] != i + 1) {
01438 for (j = 0; j < 3; j++) {
01439 atmp = V[i + 3 * j];
01440 V[i + 3 * j] = V[(rscale[i] + 3 * j) - 1];
01441 V[(rscale[i] + 3 * j) - 1] = atmp;
01442 }
01443 }
01444 }
01445 }
01446
01447 if (ihi < 3) {
01448 while (ihi + 1 < 4) {
01449 if (rscale[ihi] != ihi + 1) {
01450 for (j = 0; j < 3; j++) {
01451 atmp = V[ihi + 3 * j];
01452 V[ihi + 3 * j] = V[(rscale[ihi] + 3 * j) - 1];
01453 V[(rscale[ihi] + 3 * j) - 1] = atmp;
01454 }
01455 }
01456
01457 ihi++;
01458 }
01459 }
01460
01461 for (ii = 0; ii < 3; ii++) {
01462 absxk = fabs(V[3 * ii].re) + fabs(V[3 * ii].im);
01463 for (nzcount = 0; nzcount < 2; nzcount++) {
01464 ctoc = fabs(V[(nzcount + 3 * ii) + 1].re) + fabs(V[(nzcount + 3 * ii)
01465 + 1].im);
01466 if (ctoc > absxk) {
01467 absxk = ctoc;
01468 }
01469 }
01470
01471 if (absxk >= 6.7178761075670888E-139) {
01472 absxk = 1.0 / absxk;
01473 for (nzcount = 0; nzcount < 3; nzcount++) {
01474 V[nzcount + 3 * ii].re *= absxk;
01475 V[nzcount + 3 * ii].im *= absxk;
01476 }
01477 }
01478 }
01479
01480 if (ilascl) {
01481 notdone = TRUE;
01482 while (notdone) {
01483 cfrom1 = anrmto * 2.0041683600089728E-292;
01484 cto1 = anrm / 4.9896007738368E+291;
01485 if ((cfrom1 > anrm) && (anrm != 0.0)) {
01486 mul = 2.0041683600089728E-292;
01487 anrmto = cfrom1;
01488 } else if (cto1 > anrmto) {
01489 mul = 4.9896007738368E+291;
01490 anrm = cto1;
01491 } else {
01492 mul = anrm / anrmto;
01493 notdone = FALSE;
01494 }
01495
01496 for (ii = 0; ii < 3; ii++) {
01497 alpha1[ii].re *= mul;
01498 alpha1[ii].im *= mul;
01499 }
01500 }
01501 }
01502 }
01503 }
01504 }
01505
01506 static void eml_matlab_zhgeqz(creal_T A[9], int32_T ilo, int32_T ihi, creal_T Z
01507 [9], real_T *info, creal_T alpha1[3], creal_T beta1[3])
01508 {
01509 int32_T i;
01510 real_T eshift_re;
01511 real_T eshift_im;
01512 creal_T ctemp;
01513 real_T rho_re;
01514 real_T rho_im;
01515 real_T anorm;
01516 real_T scale;
01517 real_T sumsq;
01518 boolean_T firstNonZero;
01519 int32_T j;
01520 int32_T c;
01521 real_T reAij;
01522 real_T imAij;
01523 real_T temp2;
01524 real_T b_atol;
01525 real_T ascale;
01526 boolean_T failed;
01527 boolean_T guard1 = FALSE;
01528 boolean_T guard2 = FALSE;
01529 int32_T ifirst;
01530 int32_T istart;
01531 int32_T ilast;
01532 int32_T ilastm1;
01533 int32_T iiter;
01534 boolean_T goto60;
01535 boolean_T goto70;
01536 boolean_T goto90;
01537 int32_T jiter;
01538 int32_T exitg1;
01539 boolean_T exitg3;
01540 boolean_T b_guard1 = FALSE;
01541 creal_T d;
01542 creal_T t1;
01543 real_T sigma1_im;
01544 int32_T jp1;
01545 boolean_T exitg2;
01546 creal_T b_A;
01547 creal_T dc0;
01548 *info = 0.0;
01549 for (i = 0; i < 3; i++) {
01550 alpha1[i].re = 0.0;
01551 alpha1[i].im = 0.0;
01552 beta1[i].re = 1.0;
01553 beta1[i].im = 0.0;
01554 }
01555
01556 eshift_re = 0.0;
01557 eshift_im = 0.0;
01558 ctemp.re = 0.0;
01559 ctemp.im = 0.0;
01560 rho_re = 0.0;
01561 rho_im = 0.0;
01562 anorm = 0.0;
01563 if (ilo > ihi) {
01564 } else {
01565 scale = 0.0;
01566 sumsq = 0.0;
01567 firstNonZero = TRUE;
01568 for (j = ilo; j <= ihi; j++) {
01569 c = j + 1;
01570 if (ihi < c) {
01571 c = ihi;
01572 }
01573
01574 for (i = ilo; i <= c; i++) {
01575 reAij = A[(i + 3 * (j - 1)) - 1].re;
01576 imAij = A[(i + 3 * (j - 1)) - 1].im;
01577 if (reAij != 0.0) {
01578 anorm = fabs(reAij);
01579 if (firstNonZero) {
01580 sumsq = 1.0;
01581 scale = anorm;
01582 firstNonZero = FALSE;
01583 } else if (scale < anorm) {
01584 temp2 = scale / anorm;
01585 sumsq = 1.0 + sumsq * temp2 * temp2;
01586 scale = anorm;
01587 } else {
01588 temp2 = anorm / scale;
01589 sumsq += temp2 * temp2;
01590 }
01591 }
01592
01593 if (imAij != 0.0) {
01594 anorm = fabs(imAij);
01595 if (firstNonZero) {
01596 sumsq = 1.0;
01597 scale = anorm;
01598 firstNonZero = FALSE;
01599 } else if (scale < anorm) {
01600 temp2 = scale / anorm;
01601 sumsq = 1.0 + sumsq * temp2 * temp2;
01602 scale = anorm;
01603 } else {
01604 temp2 = anorm / scale;
01605 sumsq += temp2 * temp2;
01606 }
01607 }
01608 }
01609 }
01610
01611 anorm = scale * sqrt(sumsq);
01612 }
01613
01614 reAij = 2.2204460492503131E-16 * anorm;
01615 b_atol = 2.2250738585072014E-308;
01616 if (reAij > 2.2250738585072014E-308) {
01617 b_atol = reAij;
01618 }
01619
01620 reAij = 2.2250738585072014E-308;
01621 if (anorm > 2.2250738585072014E-308) {
01622 reAij = anorm;
01623 }
01624
01625 ascale = 1.0 / reAij;
01626 failed = TRUE;
01627 for (j = ihi; j + 1 < 4; j++) {
01628 alpha1[j] = A[j + 3 * j];
01629 }
01630
01631 guard1 = FALSE;
01632 guard2 = FALSE;
01633 if (ihi >= ilo) {
01634 ifirst = ilo;
01635 istart = ilo;
01636 ilast = ihi - 1;
01637 ilastm1 = ihi - 2;
01638 iiter = 0;
01639 goto60 = FALSE;
01640 goto70 = FALSE;
01641 goto90 = FALSE;
01642 jiter = 1;
01643 do {
01644 exitg1 = 0;
01645 if (jiter <= 30 * ((ihi - ilo) + 1)) {
01646 if (ilast + 1 == ilo) {
01647 goto60 = TRUE;
01648 } else if (fabs(A[ilast + 3 * ilastm1].re) + fabs(A[ilast + 3 * ilastm1]
01649 .im) <= b_atol) {
01650 A[ilast + 3 * ilastm1].re = 0.0;
01651 A[ilast + 3 * ilastm1].im = 0.0;
01652 goto60 = TRUE;
01653 } else {
01654 j = ilastm1;
01655 exitg3 = FALSE;
01656 while ((exitg3 == 0U) && (j + 1 >= ilo)) {
01657 i = j - 1;
01658 if (j + 1 == ilo) {
01659 firstNonZero = TRUE;
01660 } else if (fabs(A[j + 3 * i].re) + fabs(A[j + 3 * i].im) <= b_atol)
01661 {
01662 A[j + 3 * i].re = 0.0;
01663 A[j + 3 * i].im = 0.0;
01664 firstNonZero = TRUE;
01665 } else {
01666 firstNonZero = FALSE;
01667 }
01668
01669 if (firstNonZero) {
01670 ifirst = j + 1;
01671 goto70 = TRUE;
01672 exitg3 = TRUE;
01673 } else {
01674 j = i;
01675 }
01676 }
01677 }
01678
01679 if (goto60 || goto70) {
01680 firstNonZero = TRUE;
01681 } else {
01682 firstNonZero = FALSE;
01683 }
01684
01685 if (!firstNonZero) {
01686 for (i = 0; i < 3; i++) {
01687 alpha1[i].re = rtNaN;
01688 alpha1[i].im = 0.0;
01689 beta1[i].re = rtNaN;
01690 beta1[i].im = 0.0;
01691 }
01692
01693 for (i = 0; i < 3; i++) {
01694 for (c = 0; c < 3; c++) {
01695 Z[c + 3 * i].re = rtNaN;
01696 Z[c + 3 * i].im = 0.0;
01697 }
01698 }
01699
01700 *info = -1.0;
01701 exitg1 = 1;
01702 } else {
01703 b_guard1 = FALSE;
01704 if (goto60) {
01705 goto60 = FALSE;
01706 alpha1[ilast] = A[ilast + 3 * ilast];
01707 ilast = ilastm1;
01708 ilastm1--;
01709 if (ilast + 1 < ilo) {
01710 failed = FALSE;
01711 guard2 = TRUE;
01712 exitg1 = 1;
01713 } else {
01714 iiter = 0;
01715 eshift_re = 0.0;
01716 eshift_im = 0.0;
01717 b_guard1 = TRUE;
01718 }
01719 } else {
01720 if (goto70) {
01721 goto70 = FALSE;
01722 iiter++;
01723 if (iiter - div_s32_floor(iiter, 10) * 10 != 0) {
01724 d.re = -(A[ilast + 3 * ilast].re - A[ilastm1 + 3 * ilastm1].re);
01725 d.im = -(A[ilast + 3 * ilast].im - A[ilastm1 + 3 * ilastm1].im);
01726 t1 = b_eml_div(d, 2.0);
01727 anorm = A[ilastm1 + 3 * ilast].re * A[ilast + 3 * ilastm1].re -
01728 A[ilastm1 + 3 * ilast].im * A[ilast + 3 * ilastm1].im;
01729 reAij = A[ilastm1 + 3 * ilast].re * A[ilast + 3 * ilastm1].im +
01730 A[ilastm1 + 3 * ilast].im * A[ilast + 3 * ilastm1].re;
01731 d.re = (t1.re * t1.re - t1.im * t1.im) + anorm;
01732 d.im = (t1.re * t1.im + t1.im * t1.re) + reAij;
01733 b_sqrt(&d);
01734 temp2 = A[ilastm1 + 3 * ilastm1].re - (t1.re - d.re);
01735 sigma1_im = A[ilastm1 + 3 * ilastm1].im - (t1.im - d.im);
01736 scale = A[ilastm1 + 3 * ilastm1].re - (t1.re + d.re);
01737 sumsq = A[ilastm1 + 3 * ilastm1].im - (t1.im + d.im);
01738 rho_re = temp2 - A[ilast + 3 * ilast].re;
01739 rho_im = sigma1_im - A[ilast + 3 * ilast].im;
01740 anorm = scale - A[ilast + 3 * ilast].re;
01741 reAij = sumsq - A[ilast + 3 * ilast].im;
01742 if (rt_hypotd_snf(fabs(rho_re), fabs(rho_im)) <= rt_hypotd_snf
01743 (fabs(anorm), fabs(reAij))) {
01744 scale = temp2;
01745 sumsq = sigma1_im;
01746 rho_re = t1.re - d.re;
01747 rho_im = t1.im - d.im;
01748 } else {
01749 rho_re = t1.re + d.re;
01750 rho_im = t1.im + d.im;
01751 }
01752 } else {
01753 eshift_re += A[ilast + 3 * ilastm1].re;
01754 eshift_im += A[ilast + 3 * ilastm1].im;
01755 scale = eshift_re;
01756 sumsq = eshift_im;
01757 }
01758
01759 j = ilastm1;
01760 jp1 = ilastm1 + 1;
01761 exitg2 = FALSE;
01762 while ((exitg2 == 0U) && (j + 1 > ifirst)) {
01763 i = j - 1;
01764 istart = j + 1;
01765 ctemp.re = A[j + 3 * j].re - scale;
01766 ctemp.im = A[j + 3 * j].im - sumsq;
01767 anorm = ascale * (fabs(ctemp.re) + fabs(ctemp.im));
01768 temp2 = ascale * (fabs(A[jp1 + 3 * j].re) + fabs(A[jp1 + 3 * j].
01769 im));
01770 reAij = anorm;
01771 if (temp2 > anorm) {
01772 reAij = temp2;
01773 }
01774
01775 if ((reAij < 1.0) && (reAij != 0.0)) {
01776 anorm /= reAij;
01777 temp2 /= reAij;
01778 }
01779
01780 if ((fabs(A[j + 3 * i].re) + fabs(A[j + 3 * i].im)) * temp2 <=
01781 anorm * b_atol) {
01782 goto90 = TRUE;
01783 exitg2 = TRUE;
01784 } else {
01785 jp1 = j;
01786 j = i;
01787 }
01788 }
01789
01790 if (!goto90) {
01791 istart = ifirst;
01792 if (ifirst == ilastm1 + 1) {
01793 ctemp.re = rho_re;
01794 ctemp.im = rho_im;
01795 } else {
01796 ctemp.re = A[(ifirst + 3 * (ifirst - 1)) - 1].re - scale;
01797 ctemp.im = A[(ifirst + 3 * (ifirst - 1)) - 1].im - sumsq;
01798 }
01799
01800 goto90 = TRUE;
01801 }
01802 }
01803
01804 if (goto90) {
01805 goto90 = FALSE;
01806 d = A[istart + 3 * (istart - 1)];
01807 b_eml_matlab_zlartg(ctemp, d, &imAij, &t1);
01808 j = istart - 1;
01809 i = istart - 2;
01810 while (j + 1 < ilast + 1) {
01811 jp1 = j + 1;
01812 if (j + 1 > istart) {
01813 d = A[1 + 3 * i];
01814 b_A = A[jp1 + 3 * i];
01815 eml_matlab_zlartg(d, b_A, &imAij, &t1, &dc0);
01816 A[1 + 3 * i] = dc0;
01817 A[jp1 + 3 * i].re = 0.0;
01818 A[jp1 + 3 * i].im = 0.0;
01819 }
01820
01821 for (c = j; c + 1 < 4; c++) {
01822 d.re = imAij * A[j + 3 * c].re;
01823 d.im = imAij * A[j + 3 * c].im;
01824 temp2 = t1.re * A[jp1 + 3 * c].re - t1.im * A[jp1 + 3 * c].im;
01825 sigma1_im = t1.re * A[jp1 + 3 * c].im + t1.im * A[jp1 + 3 * c]
01826 .re;
01827 anorm = A[j + 3 * c].re;
01828 reAij = A[j + 3 * c].im;
01829 scale = A[j + 3 * c].im;
01830 sumsq = A[j + 3 * c].re;
01831 A[jp1 + 3 * c].re = imAij * A[jp1 + 3 * c].re - (t1.re * anorm
01832 + t1.im * reAij);
01833 A[jp1 + 3 * c].im = imAij * A[jp1 + 3 * c].im - (t1.re * scale
01834 - t1.im * sumsq);
01835 A[j + 3 * c].re = d.re + temp2;
01836 A[j + 3 * c].im = d.im + sigma1_im;
01837 }
01838
01839 t1.re = -t1.re;
01840 t1.im = -t1.im;
01841 c = jp1 + 2;
01842 if (ilast + 1 < c) {
01843 c = ilast + 1;
01844 }
01845
01846 for (i = 0; i + 1 <= c; i++) {
01847 d.re = imAij * A[i + 3 * jp1].re;
01848 d.im = imAij * A[i + 3 * jp1].im;
01849 temp2 = t1.re * A[i + 3 * j].re - t1.im * A[i + 3 * j].im;
01850 sigma1_im = t1.re * A[i + 3 * j].im + t1.im * A[i + 3 * j].re;
01851 anorm = A[i + 3 * jp1].re;
01852 reAij = A[i + 3 * jp1].im;
01853 scale = A[i + 3 * jp1].im;
01854 sumsq = A[i + 3 * jp1].re;
01855 A[i + 3 * j].re = imAij * A[i + 3 * j].re - (t1.re * anorm +
01856 t1.im * reAij);
01857 A[i + 3 * j].im = imAij * A[i + 3 * j].im - (t1.re * scale -
01858 t1.im * sumsq);
01859 A[i + 3 * jp1].re = d.re + temp2;
01860 A[i + 3 * jp1].im = d.im + sigma1_im;
01861 }
01862
01863 for (i = 0; i < 3; i++) {
01864 d.re = imAij * Z[i + 3 * jp1].re;
01865 d.im = imAij * Z[i + 3 * jp1].im;
01866 temp2 = t1.re * Z[i + 3 * j].re - t1.im * Z[i + 3 * j].im;
01867 sigma1_im = t1.re * Z[i + 3 * j].im + t1.im * Z[i + 3 * j].re;
01868 anorm = Z[i + 3 * jp1].re;
01869 reAij = Z[i + 3 * jp1].im;
01870 scale = Z[i + 3 * jp1].im;
01871 sumsq = Z[i + 3 * jp1].re;
01872 Z[i + 3 * j].re = imAij * Z[i + 3 * j].re - (t1.re * anorm +
01873 t1.im * reAij);
01874 Z[i + 3 * j].im = imAij * Z[i + 3 * j].im - (t1.re * scale -
01875 t1.im * sumsq);
01876 Z[i + 3 * jp1].re = d.re + temp2;
01877 Z[i + 3 * jp1].im = d.im + sigma1_im;
01878 }
01879
01880 i = j;
01881 j = jp1;
01882 }
01883 }
01884
01885 b_guard1 = TRUE;
01886 }
01887
01888 if (b_guard1 == TRUE) {
01889 jiter++;
01890 }
01891 }
01892 } else {
01893 guard2 = TRUE;
01894 exitg1 = 1;
01895 }
01896 } while (exitg1 == 0U);
01897 } else {
01898 guard1 = TRUE;
01899 }
01900
01901 if (guard2 == TRUE) {
01902 if (failed) {
01903 *info = (real_T)(ilast + 1);
01904 for (i = 0; i + 1 <= ilast + 1; i++) {
01905 alpha1[i].re = rtNaN;
01906 alpha1[i].im = 0.0;
01907 beta1[i].re = rtNaN;
01908 beta1[i].im = 0.0;
01909 }
01910
01911 for (i = 0; i < 3; i++) {
01912 for (c = 0; c < 3; c++) {
01913 Z[c + 3 * i].re = rtNaN;
01914 Z[c + 3 * i].im = 0.0;
01915 }
01916 }
01917 } else {
01918 guard1 = TRUE;
01919 }
01920 }
01921
01922 if (guard1 == TRUE) {
01923 for (j = 0; j + 1 <= ilo - 1; j++) {
01924 alpha1[j] = A[j + 3 * j];
01925 }
01926 }
01927 }
01928
01929 static void eml_matlab_zlartg(const creal_T f, const creal_T g, real_T *cs,
01930 creal_T *sn, creal_T *r)
01931 {
01932 real_T scale;
01933 real_T f2s;
01934 real_T g2;
01935 real_T fs_re;
01936 real_T fs_im;
01937 real_T gs_re;
01938 real_T gs_im;
01939 int32_T count;
01940 int32_T rescaledir;
01941 boolean_T guard1 = FALSE;
01942 real_T g2s;
01943 scale = fabs(f.re);
01944 f2s = fabs(f.im);
01945 if (f2s > scale) {
01946 scale = f2s;
01947 }
01948
01949 f2s = fabs(g.re);
01950 g2 = fabs(g.im);
01951 if (g2 > f2s) {
01952 f2s = g2;
01953 }
01954
01955 if (f2s > scale) {
01956 scale = f2s;
01957 }
01958
01959 fs_re = f.re;
01960 fs_im = f.im;
01961 gs_re = g.re;
01962 gs_im = g.im;
01963 count = 0;
01964 rescaledir = 0;
01965 guard1 = FALSE;
01966 if (scale >= 7.4428285367870146E+137) {
01967 do {
01968 count++;
01969 fs_re *= 1.3435752215134178E-138;
01970 fs_im *= 1.3435752215134178E-138;
01971 gs_re *= 1.3435752215134178E-138;
01972 gs_im *= 1.3435752215134178E-138;
01973 scale *= 1.3435752215134178E-138;
01974 } while (!(scale < 7.4428285367870146E+137));
01975
01976 rescaledir = 1;
01977 guard1 = TRUE;
01978 } else if (scale <= 1.3435752215134178E-138) {
01979 if ((g.re == 0.0) && (g.im == 0.0)) {
01980 *cs = 1.0;
01981 sn->re = 0.0;
01982 sn->im = 0.0;
01983 *r = f;
01984 } else {
01985 do {
01986 count++;
01987 fs_re *= 7.4428285367870146E+137;
01988 fs_im *= 7.4428285367870146E+137;
01989 gs_re *= 7.4428285367870146E+137;
01990 gs_im *= 7.4428285367870146E+137;
01991 scale *= 7.4428285367870146E+137;
01992 } while (!(scale > 1.3435752215134178E-138));
01993
01994 rescaledir = -1;
01995 guard1 = TRUE;
01996 }
01997 } else {
01998 guard1 = TRUE;
01999 }
02000
02001 if (guard1 == TRUE) {
02002 scale = fs_re * fs_re + fs_im * fs_im;
02003 g2 = gs_re * gs_re + gs_im * gs_im;
02004 f2s = g2;
02005 if (1.0 > g2) {
02006 f2s = 1.0;
02007 }
02008
02009 if (scale <= f2s * 2.0041683600089728E-292) {
02010 if ((f.re == 0.0) && (f.im == 0.0)) {
02011 *cs = 0.0;
02012 r->re = rt_hypotd_snf(fabs(g.re), fabs(g.im));
02013 r->im = 0.0;
02014 f2s = rt_hypotd_snf(fabs(gs_re), fabs(gs_im));
02015 sn->re = gs_re / f2s;
02016 sn->im = -gs_im / f2s;
02017 } else {
02018 g2s = sqrt(g2);
02019 *cs = rt_hypotd_snf(fabs(fs_re), fabs(fs_im)) / g2s;
02020 f2s = fabs(f.re);
02021 g2 = fabs(f.im);
02022 if (g2 > f2s) {
02023 f2s = g2;
02024 }
02025
02026 if (f2s > 1.0) {
02027 f2s = rt_hypotd_snf(fabs(f.re), fabs(f.im));
02028 fs_re = f.re / f2s;
02029 fs_im = f.im / f2s;
02030 } else {
02031 g2 = 7.4428285367870146E+137 * f.re;
02032 scale = 7.4428285367870146E+137 * f.im;
02033 f2s = rt_hypotd_snf(fabs(g2), fabs(scale));
02034 fs_re = g2 / f2s;
02035 fs_im = scale / f2s;
02036 }
02037
02038 gs_re /= g2s;
02039 gs_im = -gs_im / g2s;
02040 sn->re = fs_re * gs_re - fs_im * gs_im;
02041 sn->im = fs_re * gs_im + fs_im * gs_re;
02042 r->re = *cs * f.re + (sn->re * g.re - sn->im * g.im);
02043 r->im = *cs * f.im + (sn->re * g.im + sn->im * g.re);
02044 }
02045 } else {
02046 f2s = sqrt(1.0 + g2 / scale);
02047 r->re = f2s * fs_re;
02048 r->im = f2s * fs_im;
02049 *cs = 1.0 / f2s;
02050 f2s = scale + g2;
02051 g2 = r->re / f2s;
02052 f2s = r->im / f2s;
02053 sn->re = g2 * gs_re - f2s * -gs_im;
02054 sn->im = g2 * -gs_im + f2s * gs_re;
02055 if (rescaledir > 0) {
02056 for (rescaledir = 1; rescaledir <= count; rescaledir++) {
02057 r->re *= 7.4428285367870146E+137;
02058 r->im *= 7.4428285367870146E+137;
02059 }
02060 } else {
02061 if (rescaledir < 0) {
02062 for (rescaledir = 1; rescaledir <= count; rescaledir++) {
02063 r->re *= 1.3435752215134178E-138;
02064 r->im *= 1.3435752215134178E-138;
02065 }
02066 }
02067 }
02068 }
02069 }
02070 }
02071
02072 static void eml_matlab_ztgevc(const creal_T A[9], creal_T V[9])
02073 {
02074 creal_T work1[3];
02075 creal_T work2[3];
02076 real_T rworka[3];
02077 int32_T i;
02078 real_T anorm;
02079 int32_T j;
02080 real_T y;
02081 real_T xmx;
02082 real_T ascale;
02083 int32_T je;
02084 real_T temp;
02085 real_T salpha_re;
02086 real_T salpha_im;
02087 real_T acoeff;
02088 boolean_T b0;
02089 boolean_T b1;
02090 real_T scale;
02091 real_T acoefa;
02092 int32_T jr;
02093 real_T dmin;
02094 creal_T d;
02095 creal_T b_work1;
02096 for (i = 0; i < 3; i++) {
02097 work1[i].re = 0.0;
02098 work1[i].im = 0.0;
02099 work2[i].re = 0.0;
02100 work2[i].im = 0.0;
02101 rworka[i] = 0.0;
02102 }
02103
02104 anorm = fabs(A[0].re) + fabs(A[0].im);
02105 for (j = 0; j < 2; j++) {
02106 for (i = 0; i <= j; i++) {
02107 rworka[1 + j] += fabs(A[i + 3 * (1 + j)].re) + fabs(A[i + 3 * (1 + j)].im);
02108 }
02109
02110 y = rworka[1 + j] + (fabs(A[(j + 3 * (1 + j)) + 1].re) + fabs(A[(j + 3 * (1
02111 + j)) + 1].im));
02112 if (y > anorm) {
02113 anorm = y;
02114 }
02115 }
02116
02117 xmx = anorm;
02118 if (2.2250738585072014E-308 > anorm) {
02119 xmx = 2.2250738585072014E-308;
02120 }
02121
02122 ascale = 1.0 / xmx;
02123 for (je = 0; je < 3; je++) {
02124 y = (fabs(A[(3 * (2 - je) - je) + 2].re) + fabs(A[(3 * (2 - je) - je) + 2].
02125 im)) * ascale;
02126 if (1.0 > y) {
02127 y = 1.0;
02128 }
02129
02130 temp = 1.0 / y;
02131 salpha_re = ascale * (temp * A[(3 * (2 - je) - je) + 2].re);
02132 salpha_im = ascale * (temp * A[(3 * (2 - je) - je) + 2].im);
02133 acoeff = temp * ascale;
02134 if ((fabs(temp) >= 2.2250738585072014E-308) && (fabs(acoeff) <
02135 3.0062525400134592E-292)) {
02136 b0 = TRUE;
02137 } else {
02138 b0 = FALSE;
02139 }
02140
02141 if ((fabs(salpha_re) + fabs(salpha_im) >= 2.2250738585072014E-308) && (fabs
02142 (salpha_re) + fabs(salpha_im) < 3.0062525400134592E-292)) {
02143 b1 = TRUE;
02144 } else {
02145 b1 = FALSE;
02146 }
02147
02148 scale = 1.0;
02149 if (b0) {
02150 xmx = anorm;
02151 if (3.3264005158911995E+291 < anorm) {
02152 xmx = 3.3264005158911995E+291;
02153 }
02154
02155 scale = 3.0062525400134592E-292 / fabs(temp) * xmx;
02156 }
02157
02158 if (b1) {
02159 xmx = 3.0062525400134592E-292 / (fabs(salpha_re) + fabs(salpha_im));
02160 if (xmx > scale) {
02161 scale = xmx;
02162 }
02163 }
02164
02165 if (b0 || b1) {
02166 y = fabs(acoeff);
02167 xmx = fabs(salpha_re) + fabs(salpha_im);
02168 if (1.0 > y) {
02169 y = 1.0;
02170 }
02171
02172 if (xmx > y) {
02173 y = xmx;
02174 }
02175
02176 y = 1.0 / (2.2250738585072014E-308 * y);
02177 if (y < scale) {
02178 scale = y;
02179 }
02180
02181 if (b0) {
02182 acoeff = ascale * (scale * temp);
02183 } else {
02184 acoeff *= scale;
02185 }
02186
02187 if (b1) {
02188 salpha_re *= scale;
02189 salpha_im *= scale;
02190 } else {
02191 salpha_re *= scale;
02192 salpha_im *= scale;
02193 }
02194 }
02195
02196 acoefa = fabs(acoeff);
02197 for (jr = 0; jr < 3; jr++) {
02198 work1[jr].re = 0.0;
02199 work1[jr].im = 0.0;
02200 }
02201
02202 work1[2 - je].re = 1.0;
02203 work1[2 - je].im = 0.0;
02204 dmin = 2.2204460492503131E-16 * acoefa * anorm;
02205 y = 2.2204460492503131E-16 * (fabs(salpha_re) + fabs(salpha_im));
02206 if (y > dmin) {
02207 dmin = y;
02208 }
02209
02210 if (2.2250738585072014E-308 > dmin) {
02211 dmin = 2.2250738585072014E-308;
02212 }
02213
02214 for (jr = 0; jr <= 1 - je; jr++) {
02215 work1[jr].re = acoeff * A[jr + 3 * (2 - je)].re;
02216 work1[jr].im = acoeff * A[jr + 3 * (2 - je)].im;
02217 }
02218
02219 work1[2 - je].re = 1.0;
02220 work1[2 - je].im = 0.0;
02221 for (j = 0; j <= 1 - je; j++) {
02222 i = (-je - j) + 1;
02223 d.re = acoeff * A[i + 3 * i].re - salpha_re;
02224 d.im = acoeff * A[i + 3 * i].im - salpha_im;
02225 if (fabs(d.re) + fabs(d.im) <= dmin) {
02226 d.re = dmin;
02227 d.im = 0.0;
02228 }
02229
02230 if ((fabs(d.re) + fabs(d.im) < 1.0) && (fabs(work1[i].re) + fabs(work1[i].
02231 im) >= 1.4980776123852632E+307 * (fabs(d.re) + fabs(d.im)))) {
02232 temp = 1.0 / (fabs(work1[i].re) + fabs(work1[i].im));
02233 for (jr = 0; jr <= 2 - je; jr++) {
02234 work1[jr].re *= temp;
02235 work1[jr].im *= temp;
02236 }
02237 }
02238
02239 b_work1.re = -work1[i].re;
02240 b_work1.im = -work1[i].im;
02241 work1[i] = c_eml_div(b_work1, d);
02242 if (i + 1 > 1) {
02243 if (fabs(work1[1].re) + fabs(work1[1].im) > 1.0) {
02244 temp = 1.0 / (fabs(work1[1].re) + fabs(work1[1].im));
02245 if (acoefa * rworka[1] >= 1.4980776123852632E+307 * temp) {
02246 for (jr = 0; jr <= 2 - je; jr++) {
02247 work1[jr].re *= temp;
02248 work1[jr].im *= temp;
02249 }
02250 }
02251 }
02252
02253 xmx = acoeff * work1[1].re;
02254 scale = acoeff * work1[1].im;
02255 work1[0].re += xmx * A[3].re - scale * A[3].im;
02256 work1[0].im += xmx * A[3].im + scale * A[3].re;
02257 }
02258 }
02259
02260 for (jr = 0; jr < 3; jr++) {
02261 work2[jr].re = 0.0;
02262 work2[jr].im = 0.0;
02263 }
02264
02265 for (i = 0; i <= 2 - je; i++) {
02266 for (jr = 0; jr < 3; jr++) {
02267 xmx = V[jr + 3 * i].re * work1[i].re - V[jr + 3 * i].im * work1[i].im;
02268 scale = V[jr + 3 * i].re * work1[i].im + V[jr + 3 * i].im * work1[i].re;
02269 work2[jr].re += xmx;
02270 work2[jr].im += scale;
02271 }
02272 }
02273
02274 xmx = fabs(work2[0].re) + fabs(work2[0].im);
02275 for (jr = 0; jr < 2; jr++) {
02276 y = fabs(work2[1 + jr].re) + fabs(work2[1 + jr].im);
02277 if (y > xmx) {
02278 xmx = y;
02279 }
02280 }
02281
02282 if (xmx > 2.2250738585072014E-308) {
02283 temp = 1.0 / xmx;
02284 for (jr = 0; jr < 3; jr++) {
02285 V[jr + 3 * (2 - je)].re = temp * work2[jr].re;
02286 V[jr + 3 * (2 - je)].im = temp * work2[jr].im;
02287 }
02288 } else {
02289 for (jr = 0; jr < 3; jr++) {
02290 V[jr + 3 * (2 - je)].re = 0.0;
02291 V[jr + 3 * (2 - je)].im = 0.0;
02292 }
02293 }
02294 }
02295 }
02296
02297 void b_eig(const real_T A[4], creal_T V[4], creal_T D[4])
02298 {
02299 creal_T b_A[4];
02300 int32_T j;
02301 creal_T beta1[2];
02302 creal_T alpha1[2];
02303 real_T colnorm;
02304 int32_T coltop;
02305 real_T scale;
02306 real_T absxk;
02307 real_T t;
02308 real_T alpha1_im;
02309 for (j = 0; j < 4; j++) {
02310 b_A[j].re = A[j];
02311 b_A[j].im = 0.0;
02312 }
02313
02314 b_eml_matlab_zggev(b_A, &colnorm, alpha1, beta1, V);
02315 for (coltop = 0; coltop < 4; coltop += 2) {
02316 colnorm = 0.0;
02317 scale = 2.2250738585072014E-308;
02318 for (j = coltop; j + 1 <= coltop + 2; j++) {
02319 absxk = fabs(V[j].re);
02320 if (absxk > scale) {
02321 t = scale / absxk;
02322 colnorm = 1.0 + colnorm * t * t;
02323 scale = absxk;
02324 } else {
02325 t = absxk / scale;
02326 colnorm += t * t;
02327 }
02328
02329 absxk = fabs(V[j].im);
02330 if (absxk > scale) {
02331 t = scale / absxk;
02332 colnorm = 1.0 + colnorm * t * t;
02333 scale = absxk;
02334 } else {
02335 t = absxk / scale;
02336 colnorm += t * t;
02337 }
02338 }
02339
02340 colnorm = scale * sqrt(colnorm);
02341 for (j = coltop; j + 1 <= coltop + 2; j++) {
02342 V[j] = b_eml_div(V[j], colnorm);
02343 }
02344 }
02345
02346 for (j = 0; j < 2; j++) {
02347 t = alpha1[j].re;
02348 alpha1_im = alpha1[j].im;
02349 if (beta1[j].im == 0.0) {
02350 if (alpha1[j].im == 0.0) {
02351 alpha1[j].re /= beta1[j].re;
02352 alpha1[j].im = 0.0;
02353 } else if (alpha1[j].re == 0.0) {
02354 alpha1[j].re = 0.0;
02355 alpha1[j].im = alpha1_im / beta1[j].re;
02356 } else {
02357 alpha1[j].re /= beta1[j].re;
02358 alpha1[j].im = alpha1_im / beta1[j].re;
02359 }
02360 } else if (beta1[j].re == 0.0) {
02361 if (alpha1[j].re == 0.0) {
02362 alpha1[j].re = alpha1[j].im / beta1[j].im;
02363 alpha1[j].im = 0.0;
02364 } else if (alpha1[j].im == 0.0) {
02365 alpha1[j].re = 0.0;
02366 alpha1[j].im = -(t / beta1[j].im);
02367 } else {
02368 alpha1[j].re = alpha1[j].im / beta1[j].im;
02369 alpha1[j].im = -(t / beta1[j].im);
02370 }
02371 } else {
02372 absxk = fabs(beta1[j].re);
02373 colnorm = fabs(beta1[j].im);
02374 if (absxk > colnorm) {
02375 colnorm = beta1[j].im / beta1[j].re;
02376 scale = beta1[j].re + colnorm * beta1[j].im;
02377 alpha1[j].re = (alpha1[j].re + colnorm * alpha1[j].im) / scale;
02378 alpha1[j].im = (alpha1_im - colnorm * t) / scale;
02379 } else if (colnorm == absxk) {
02380 colnorm = beta1[j].re > 0.0 ? 0.5 : -0.5;
02381 scale = beta1[j].im > 0.0 ? 0.5 : -0.5;
02382 alpha1[j].re = (alpha1[j].re * colnorm + alpha1[j].im * scale) / absxk;
02383 alpha1[j].im = (alpha1_im * colnorm - t * scale) / absxk;
02384 } else {
02385 colnorm = beta1[j].re / beta1[j].im;
02386 scale = beta1[j].im + colnorm * beta1[j].re;
02387 alpha1[j].re = (colnorm * alpha1[j].re + alpha1[j].im) / scale;
02388 alpha1[j].im = (colnorm * alpha1_im - t) / scale;
02389 }
02390 }
02391 }
02392
02393 for (j = 0; j < 4; j++) {
02394 D[j].re = 0.0;
02395 D[j].im = 0.0;
02396 }
02397
02398 for (j = 0; j < 2; j++) {
02399 D[j + (j << 1)] = alpha1[j];
02400 }
02401 }
02402
02403 void eig(const real_T A[9], creal_T V[9], creal_T D[9])
02404 {
02405 creal_T b_A[9];
02406 int32_T j;
02407 creal_T beta1[3];
02408 creal_T alpha1[3];
02409 real_T colnorm;
02410 int32_T coltop;
02411 real_T scale;
02412 real_T absxk;
02413 real_T t;
02414 real_T alpha1_im;
02415 for (j = 0; j < 9; j++) {
02416 b_A[j].re = A[j];
02417 b_A[j].im = 0.0;
02418 }
02419
02420 eml_matlab_zggev(b_A, &colnorm, alpha1, beta1, V);
02421 for (coltop = 0; coltop < 8; coltop += 3) {
02422 colnorm = 0.0;
02423 scale = 2.2250738585072014E-308;
02424 for (j = coltop; j + 1 <= coltop + 3; j++) {
02425 absxk = fabs(V[j].re);
02426 if (absxk > scale) {
02427 t = scale / absxk;
02428 colnorm = 1.0 + colnorm * t * t;
02429 scale = absxk;
02430 } else {
02431 t = absxk / scale;
02432 colnorm += t * t;
02433 }
02434
02435 absxk = fabs(V[j].im);
02436 if (absxk > scale) {
02437 t = scale / absxk;
02438 colnorm = 1.0 + colnorm * t * t;
02439 scale = absxk;
02440 } else {
02441 t = absxk / scale;
02442 colnorm += t * t;
02443 }
02444 }
02445
02446 colnorm = scale * sqrt(colnorm);
02447 for (j = coltop; j + 1 <= coltop + 3; j++) {
02448 V[j] = b_eml_div(V[j], colnorm);
02449 }
02450 }
02451
02452 for (j = 0; j < 3; j++) {
02453 t = alpha1[j].re;
02454 alpha1_im = alpha1[j].im;
02455 if (beta1[j].im == 0.0) {
02456 if (alpha1[j].im == 0.0) {
02457 alpha1[j].re /= beta1[j].re;
02458 alpha1[j].im = 0.0;
02459 } else if (alpha1[j].re == 0.0) {
02460 alpha1[j].re = 0.0;
02461 alpha1[j].im = alpha1_im / beta1[j].re;
02462 } else {
02463 alpha1[j].re /= beta1[j].re;
02464 alpha1[j].im = alpha1_im / beta1[j].re;
02465 }
02466 } else if (beta1[j].re == 0.0) {
02467 if (alpha1[j].re == 0.0) {
02468 alpha1[j].re = alpha1[j].im / beta1[j].im;
02469 alpha1[j].im = 0.0;
02470 } else if (alpha1[j].im == 0.0) {
02471 alpha1[j].re = 0.0;
02472 alpha1[j].im = -(t / beta1[j].im);
02473 } else {
02474 alpha1[j].re = alpha1[j].im / beta1[j].im;
02475 alpha1[j].im = -(t / beta1[j].im);
02476 }
02477 } else {
02478 absxk = fabs(beta1[j].re);
02479 colnorm = fabs(beta1[j].im);
02480 if (absxk > colnorm) {
02481 colnorm = beta1[j].im / beta1[j].re;
02482 scale = beta1[j].re + colnorm * beta1[j].im;
02483 alpha1[j].re = (alpha1[j].re + colnorm * alpha1[j].im) / scale;
02484 alpha1[j].im = (alpha1_im - colnorm * t) / scale;
02485 } else if (colnorm == absxk) {
02486 colnorm = beta1[j].re > 0.0 ? 0.5 : -0.5;
02487 scale = beta1[j].im > 0.0 ? 0.5 : -0.5;
02488 alpha1[j].re = (alpha1[j].re * colnorm + alpha1[j].im * scale) / absxk;
02489 alpha1[j].im = (alpha1_im * colnorm - t * scale) / absxk;
02490 } else {
02491 colnorm = beta1[j].re / beta1[j].im;
02492 scale = beta1[j].im + colnorm * beta1[j].re;
02493 alpha1[j].re = (colnorm * alpha1[j].re + alpha1[j].im) / scale;
02494 alpha1[j].im = (colnorm * alpha1_im - t) / scale;
02495 }
02496 }
02497 }
02498
02499 for (j = 0; j < 9; j++) {
02500 D[j].re = 0.0;
02501 D[j].im = 0.0;
02502 }
02503
02504 for (j = 0; j < 3; j++) {
02505 D[j + 3 * j] = alpha1[j];
02506 }
02507 }
02508
02509