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 "expm.h"
00014 #include "mldivide.h"
00015 #include "Optimal_affine_tracking_3d16_fast_realtime_rtwutil.h"
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 static void PadeApproximantOfDegree(const creal_T A[9], real_T m, creal_T F[9]);
00027 static void b_PadeApproximantOfDegree(const real_T A[9], real_T m, real_T F[9]);
00028 static void c_PadeApproximantOfDegree(const real_T A[4], real_T m, real_T F[4]);
00029 static real_T rt_powd_snf(real_T u0, real_T u1);
00030
00031
00032 static void PadeApproximantOfDegree(const creal_T A[9], real_T m, creal_T F[9])
00033 {
00034 int32_T i8;
00035 int32_T k;
00036 creal_T A2[9];
00037 int32_T i9;
00038 creal_T U[9];
00039 creal_T A4[9];
00040 creal_T V[9];
00041 real_T d;
00042 creal_T A3[9];
00043 creal_T b_A4[9];
00044 real_T uk_im;
00045 for (i8 = 0; i8 < 3; i8++) {
00046 for (k = 0; k < 3; k++) {
00047 A2[i8 + 3 * k].re = 0.0;
00048 A2[i8 + 3 * k].im = 0.0;
00049 for (i9 = 0; i9 < 3; i9++) {
00050 A2[i8 + 3 * k].re += A[i8 + 3 * i9].re * A[i9 + 3 * k].re - A[i8 + 3 *
00051 i9].im * A[i9 + 3 * k].im;
00052 A2[i8 + 3 * k].im += A[i8 + 3 * i9].re * A[i9 + 3 * k].im + A[i8 + 3 *
00053 i9].im * A[i9 + 3 * k].re;
00054 }
00055 }
00056 }
00057
00058 if (m == 3.0) {
00059 memcpy(&U[0], &A2[0], 9U * sizeof(creal_T));
00060 for (k = 0; k < 3; k++) {
00061 U[k + 3 * k].re += 60.0;
00062 }
00063
00064 for (i8 = 0; i8 < 3; i8++) {
00065 for (k = 0; k < 3; k++) {
00066 A4[i8 + 3 * k].re = 0.0;
00067 A4[i8 + 3 * k].im = 0.0;
00068 for (i9 = 0; i9 < 3; i9++) {
00069 A4[i8 + 3 * k].re += A[i8 + 3 * i9].re * U[i9 + 3 * k].re - A[i8 + 3 *
00070 i9].im * U[i9 + 3 * k].im;
00071 A4[i8 + 3 * k].im += A[i8 + 3 * i9].re * U[i9 + 3 * k].im + A[i8 + 3 *
00072 i9].im * U[i9 + 3 * k].re;
00073 }
00074 }
00075 }
00076
00077 for (i8 = 0; i8 < 3; i8++) {
00078 for (k = 0; k < 3; k++) {
00079 U[k + 3 * i8] = A4[k + 3 * i8];
00080 }
00081 }
00082
00083 for (i8 = 0; i8 < 9; i8++) {
00084 V[i8].re = 12.0 * A2[i8].re;
00085 V[i8].im = 12.0 * A2[i8].im;
00086 }
00087
00088 d = 120.0;
00089 } else {
00090 for (i8 = 0; i8 < 3; i8++) {
00091 for (k = 0; k < 3; k++) {
00092 A3[i8 + 3 * k].re = 0.0;
00093 A3[i8 + 3 * k].im = 0.0;
00094 for (i9 = 0; i9 < 3; i9++) {
00095 A3[i8 + 3 * k].re += A2[i8 + 3 * i9].re * A2[i9 + 3 * k].re - A2[i8 +
00096 3 * i9].im * A2[i9 + 3 * k].im;
00097 A3[i8 + 3 * k].im += A2[i8 + 3 * i9].re * A2[i9 + 3 * k].im + A2[i8 +
00098 3 * i9].im * A2[i9 + 3 * k].re;
00099 }
00100 }
00101 }
00102
00103 if (m == 5.0) {
00104 for (i8 = 0; i8 < 9; i8++) {
00105 U[i8].re = A3[i8].re + 420.0 * A2[i8].re;
00106 U[i8].im = A3[i8].im + 420.0 * A2[i8].im;
00107 }
00108
00109 for (k = 0; k < 3; k++) {
00110 U[k + 3 * k].re += 15120.0;
00111 }
00112
00113 for (i8 = 0; i8 < 3; i8++) {
00114 for (k = 0; k < 3; k++) {
00115 A4[i8 + 3 * k].re = 0.0;
00116 A4[i8 + 3 * k].im = 0.0;
00117 for (i9 = 0; i9 < 3; i9++) {
00118 A4[i8 + 3 * k].re += A[i8 + 3 * i9].re * U[i9 + 3 * k].re - A[i8 + 3
00119 * i9].im * U[i9 + 3 * k].im;
00120 A4[i8 + 3 * k].im += A[i8 + 3 * i9].re * U[i9 + 3 * k].im + A[i8 + 3
00121 * i9].im * U[i9 + 3 * k].re;
00122 }
00123 }
00124 }
00125
00126 for (i8 = 0; i8 < 3; i8++) {
00127 for (k = 0; k < 3; k++) {
00128 U[k + 3 * i8] = A4[k + 3 * i8];
00129 }
00130 }
00131
00132 for (i8 = 0; i8 < 9; i8++) {
00133 V[i8].re = 30.0 * A3[i8].re + 3360.0 * A2[i8].re;
00134 V[i8].im = 30.0 * A3[i8].im + 3360.0 * A2[i8].im;
00135 }
00136
00137 d = 30240.0;
00138 } else {
00139 for (i8 = 0; i8 < 3; i8++) {
00140 for (k = 0; k < 3; k++) {
00141 b_A4[i8 + 3 * k].re = 0.0;
00142 b_A4[i8 + 3 * k].im = 0.0;
00143 for (i9 = 0; i9 < 3; i9++) {
00144 b_A4[i8 + 3 * k].re += A3[i8 + 3 * i9].re * A2[i9 + 3 * k].re -
00145 A3[i8 + 3 * i9].im * A2[i9 + 3 * k].im;
00146 b_A4[i8 + 3 * k].im += A3[i8 + 3 * i9].re * A2[i9 + 3 * k].im +
00147 A3[i8 + 3 * i9].im * A2[i9 + 3 * k].re;
00148 }
00149 }
00150 }
00151
00152 if (m == 7.0) {
00153 for (i8 = 0; i8 < 9; i8++) {
00154 U[i8].re = (b_A4[i8].re + 1512.0 * A3[i8].re) + 277200.0 * A2[i8].re;
00155 U[i8].im = (b_A4[i8].im + 1512.0 * A3[i8].im) + 277200.0 * A2[i8].im;
00156 }
00157
00158 for (k = 0; k < 3; k++) {
00159 U[k + 3 * k].re += 8.64864E+6;
00160 }
00161
00162 for (i8 = 0; i8 < 3; i8++) {
00163 for (k = 0; k < 3; k++) {
00164 A4[i8 + 3 * k].re = 0.0;
00165 A4[i8 + 3 * k].im = 0.0;
00166 for (i9 = 0; i9 < 3; i9++) {
00167 A4[i8 + 3 * k].re += A[i8 + 3 * i9].re * U[i9 + 3 * k].re - A[i8 +
00168 3 * i9].im * U[i9 + 3 * k].im;
00169 A4[i8 + 3 * k].im += A[i8 + 3 * i9].re * U[i9 + 3 * k].im + A[i8 +
00170 3 * i9].im * U[i9 + 3 * k].re;
00171 }
00172 }
00173 }
00174
00175 for (i8 = 0; i8 < 3; i8++) {
00176 for (k = 0; k < 3; k++) {
00177 U[k + 3 * i8] = A4[k + 3 * i8];
00178 }
00179 }
00180
00181 for (i8 = 0; i8 < 9; i8++) {
00182 V[i8].re = (56.0 * b_A4[i8].re + 25200.0 * A3[i8].re) + 1.99584E+6 *
00183 A2[i8].re;
00184 V[i8].im = (56.0 * b_A4[i8].im + 25200.0 * A3[i8].im) + 1.99584E+6 *
00185 A2[i8].im;
00186 }
00187
00188 d = 1.729728E+7;
00189 } else if (m == 9.0) {
00190 for (i8 = 0; i8 < 3; i8++) {
00191 for (k = 0; k < 3; k++) {
00192 V[i8 + 3 * k].re = 0.0;
00193 V[i8 + 3 * k].im = 0.0;
00194 for (i9 = 0; i9 < 3; i9++) {
00195 V[i8 + 3 * k].re += b_A4[i8 + 3 * i9].re * A2[i9 + 3 * k].re -
00196 b_A4[i8 + 3 * i9].im * A2[i9 + 3 * k].im;
00197 V[i8 + 3 * k].im += b_A4[i8 + 3 * i9].re * A2[i9 + 3 * k].im +
00198 b_A4[i8 + 3 * i9].im * A2[i9 + 3 * k].re;
00199 }
00200 }
00201 }
00202
00203 for (i8 = 0; i8 < 9; i8++) {
00204 U[i8].re = ((V[i8].re + 3960.0 * b_A4[i8].re) + 2.16216E+6 * A3[i8].re)
00205 + 3.027024E+8 * A2[i8].re;
00206 U[i8].im = ((V[i8].im + 3960.0 * b_A4[i8].im) + 2.16216E+6 * A3[i8].im)
00207 + 3.027024E+8 * A2[i8].im;
00208 }
00209
00210 for (k = 0; k < 3; k++) {
00211 U[k + 3 * k].re += 8.8216128E+9;
00212 }
00213
00214 for (i8 = 0; i8 < 3; i8++) {
00215 for (k = 0; k < 3; k++) {
00216 A4[i8 + 3 * k].re = 0.0;
00217 A4[i8 + 3 * k].im = 0.0;
00218 for (i9 = 0; i9 < 3; i9++) {
00219 A4[i8 + 3 * k].re += A[i8 + 3 * i9].re * U[i9 + 3 * k].re - A[i8 +
00220 3 * i9].im * U[i9 + 3 * k].im;
00221 A4[i8 + 3 * k].im += A[i8 + 3 * i9].re * U[i9 + 3 * k].im + A[i8 +
00222 3 * i9].im * U[i9 + 3 * k].re;
00223 }
00224 }
00225 }
00226
00227 for (i8 = 0; i8 < 3; i8++) {
00228 for (k = 0; k < 3; k++) {
00229 U[k + 3 * i8] = A4[k + 3 * i8];
00230 }
00231 }
00232
00233 for (i8 = 0; i8 < 9; i8++) {
00234 V[i8].re = ((90.0 * V[i8].re + 110880.0 * b_A4[i8].re) + 3.027024E+7 *
00235 A3[i8].re) + 2.0756736E+9 * A2[i8].re;
00236 V[i8].im = ((90.0 * V[i8].im + 110880.0 * b_A4[i8].im) + 3.027024E+7 *
00237 A3[i8].im) + 2.0756736E+9 * A2[i8].im;
00238 }
00239
00240 d = 1.76432256E+10;
00241 } else {
00242 for (i8 = 0; i8 < 9; i8++) {
00243 U[i8].re = (3.352212864E+10 * b_A4[i8].re + 1.05594705216E+13 * A3[i8]
00244 .re) + 1.1873537964288E+15 * A2[i8].re;
00245 U[i8].im = (3.352212864E+10 * b_A4[i8].im + 1.05594705216E+13 * A3[i8]
00246 .im) + 1.1873537964288E+15 * A2[i8].im;
00247 }
00248
00249 for (k = 0; k < 3; k++) {
00250 U[k + 3 * k].re += 3.238237626624E+16;
00251 for (i8 = 0; i8 < 3; i8++) {
00252 A4[i8 + 3 * k].re = (b_A4[i8 + 3 * k].re + 16380.0 * A3[i8 + 3 * k].
00253 re) + 4.08408E+7 * A2[i8 + 3 * k].re;
00254 A4[i8 + 3 * k].im = (b_A4[i8 + 3 * k].im + 16380.0 * A3[i8 + 3 * k].
00255 im) + 4.08408E+7 * A2[i8 + 3 * k].im;
00256 }
00257 }
00258
00259 for (i8 = 0; i8 < 3; i8++) {
00260 for (k = 0; k < 3; k++) {
00261 d = 0.0;
00262 uk_im = 0.0;
00263 for (i9 = 0; i9 < 3; i9++) {
00264 d += b_A4[i8 + 3 * i9].re * A4[i9 + 3 * k].re - b_A4[i8 + 3 * i9].
00265 im * A4[i9 + 3 * k].im;
00266 uk_im += b_A4[i8 + 3 * i9].re * A4[i9 + 3 * k].im + b_A4[i8 + 3 *
00267 i9].im * A4[i9 + 3 * k].re;
00268 }
00269
00270 V[i8 + 3 * k].re = d + U[i8 + 3 * k].re;
00271 V[i8 + 3 * k].im = uk_im + U[i8 + 3 * k].im;
00272 }
00273 }
00274
00275 for (i8 = 0; i8 < 3; i8++) {
00276 for (k = 0; k < 3; k++) {
00277 U[i8 + 3 * k].re = 0.0;
00278 U[i8 + 3 * k].im = 0.0;
00279 for (i9 = 0; i9 < 3; i9++) {
00280 U[i8 + 3 * k].re += A[i8 + 3 * i9].re * V[i9 + 3 * k].re - A[i8 +
00281 3 * i9].im * V[i9 + 3 * k].im;
00282 U[i8 + 3 * k].im += A[i8 + 3 * i9].re * V[i9 + 3 * k].im + A[i8 +
00283 3 * i9].im * V[i9 + 3 * k].re;
00284 }
00285
00286 A4[k + 3 * i8].re = (182.0 * b_A4[k + 3 * i8].re + 960960.0 * A3[k +
00287 3 * i8].re) + 1.32324192E+9 * A2[k + 3 * i8].re;
00288 A4[k + 3 * i8].im = (182.0 * b_A4[k + 3 * i8].im + 960960.0 * A3[k +
00289 3 * i8].im) + 1.32324192E+9 * A2[k + 3 * i8].im;
00290 }
00291 }
00292
00293 for (i8 = 0; i8 < 3; i8++) {
00294 for (k = 0; k < 3; k++) {
00295 d = 0.0;
00296 uk_im = 0.0;
00297 for (i9 = 0; i9 < 3; i9++) {
00298 d += b_A4[i8 + 3 * i9].re * A4[i9 + 3 * k].re - b_A4[i8 + 3 * i9].
00299 im * A4[i9 + 3 * k].im;
00300 uk_im += b_A4[i8 + 3 * i9].re * A4[i9 + 3 * k].im + b_A4[i8 + 3 *
00301 i9].im * A4[i9 + 3 * k].re;
00302 }
00303
00304 V[i8 + 3 * k].re = ((d + 6.704425728E+11 * b_A4[i8 + 3 * k].re) +
00305 1.29060195264E+14 * A3[i8 + 3 * k].re) +
00306 7.7717703038976E+15 * A2[i8 + 3 * k].re;
00307 V[i8 + 3 * k].im = ((uk_im + 6.704425728E+11 * b_A4[i8 + 3 * k].im)
00308 + 1.29060195264E+14 * A3[i8 + 3 * k].im) +
00309 7.7717703038976E+15 * A2[i8 + 3 * k].im;
00310 }
00311 }
00312
00313 d = 6.476475253248E+16;
00314 }
00315 }
00316 }
00317
00318 for (k = 0; k < 3; k++) {
00319 V[k + 3 * k].re += d;
00320 }
00321
00322 for (k = 0; k < 9; k++) {
00323 d = U[k].re;
00324 uk_im = U[k].im;
00325 U[k].re = V[k].re - U[k].re;
00326 U[k].im = V[k].im - U[k].im;
00327 V[k].re += d;
00328 V[k].im += uk_im;
00329 }
00330
00331 mldivide(U, V, F);
00332 }
00333
00334 static void b_PadeApproximantOfDegree(const real_T A[9], real_T m, real_T F[9])
00335 {
00336 int32_T i17;
00337 int32_T k;
00338 real_T A2[9];
00339 int32_T i18;
00340 real_T U[9];
00341 real_T A4[9];
00342 real_T V[9];
00343 real_T d;
00344 real_T A3[9];
00345 real_T b_A4[9];
00346 for (i17 = 0; i17 < 3; i17++) {
00347 for (k = 0; k < 3; k++) {
00348 A2[i17 + 3 * k] = 0.0;
00349 for (i18 = 0; i18 < 3; i18++) {
00350 A2[i17 + 3 * k] += A[i17 + 3 * i18] * A[i18 + 3 * k];
00351 }
00352 }
00353 }
00354
00355 if (m == 3.0) {
00356 memcpy(&U[0], &A2[0], 9U * sizeof(real_T));
00357 for (k = 0; k < 3; k++) {
00358 U[k + 3 * k] += 60.0;
00359 }
00360
00361 for (i17 = 0; i17 < 3; i17++) {
00362 for (k = 0; k < 3; k++) {
00363 A4[i17 + 3 * k] = 0.0;
00364 for (i18 = 0; i18 < 3; i18++) {
00365 A4[i17 + 3 * k] += A[i17 + 3 * i18] * U[i18 + 3 * k];
00366 }
00367 }
00368 }
00369
00370 for (i17 = 0; i17 < 3; i17++) {
00371 for (k = 0; k < 3; k++) {
00372 U[k + 3 * i17] = A4[k + 3 * i17];
00373 }
00374 }
00375
00376 for (i17 = 0; i17 < 9; i17++) {
00377 V[i17] = 12.0 * A2[i17];
00378 }
00379
00380 d = 120.0;
00381 } else {
00382 for (i17 = 0; i17 < 3; i17++) {
00383 for (k = 0; k < 3; k++) {
00384 A3[i17 + 3 * k] = 0.0;
00385 for (i18 = 0; i18 < 3; i18++) {
00386 A3[i17 + 3 * k] += A2[i17 + 3 * i18] * A2[i18 + 3 * k];
00387 }
00388 }
00389 }
00390
00391 if (m == 5.0) {
00392 for (i17 = 0; i17 < 9; i17++) {
00393 U[i17] = A3[i17] + 420.0 * A2[i17];
00394 }
00395
00396 for (k = 0; k < 3; k++) {
00397 U[k + 3 * k] += 15120.0;
00398 }
00399
00400 for (i17 = 0; i17 < 3; i17++) {
00401 for (k = 0; k < 3; k++) {
00402 A4[i17 + 3 * k] = 0.0;
00403 for (i18 = 0; i18 < 3; i18++) {
00404 A4[i17 + 3 * k] += A[i17 + 3 * i18] * U[i18 + 3 * k];
00405 }
00406 }
00407 }
00408
00409 for (i17 = 0; i17 < 3; i17++) {
00410 for (k = 0; k < 3; k++) {
00411 U[k + 3 * i17] = A4[k + 3 * i17];
00412 }
00413 }
00414
00415 for (i17 = 0; i17 < 9; i17++) {
00416 V[i17] = 30.0 * A3[i17] + 3360.0 * A2[i17];
00417 }
00418
00419 d = 30240.0;
00420 } else {
00421 for (i17 = 0; i17 < 3; i17++) {
00422 for (k = 0; k < 3; k++) {
00423 b_A4[i17 + 3 * k] = 0.0;
00424 for (i18 = 0; i18 < 3; i18++) {
00425 b_A4[i17 + 3 * k] += A3[i17 + 3 * i18] * A2[i18 + 3 * k];
00426 }
00427 }
00428 }
00429
00430 if (m == 7.0) {
00431 for (i17 = 0; i17 < 9; i17++) {
00432 U[i17] = (b_A4[i17] + 1512.0 * A3[i17]) + 277200.0 * A2[i17];
00433 }
00434
00435 for (k = 0; k < 3; k++) {
00436 U[k + 3 * k] += 8.64864E+6;
00437 }
00438
00439 for (i17 = 0; i17 < 3; i17++) {
00440 for (k = 0; k < 3; k++) {
00441 A4[i17 + 3 * k] = 0.0;
00442 for (i18 = 0; i18 < 3; i18++) {
00443 A4[i17 + 3 * k] += A[i17 + 3 * i18] * U[i18 + 3 * k];
00444 }
00445 }
00446 }
00447
00448 for (i17 = 0; i17 < 3; i17++) {
00449 for (k = 0; k < 3; k++) {
00450 U[k + 3 * i17] = A4[k + 3 * i17];
00451 }
00452 }
00453
00454 for (i17 = 0; i17 < 9; i17++) {
00455 V[i17] = (56.0 * b_A4[i17] + 25200.0 * A3[i17]) + 1.99584E+6 * A2[i17];
00456 }
00457
00458 d = 1.729728E+7;
00459 } else if (m == 9.0) {
00460 for (i17 = 0; i17 < 3; i17++) {
00461 for (k = 0; k < 3; k++) {
00462 V[i17 + 3 * k] = 0.0;
00463 for (i18 = 0; i18 < 3; i18++) {
00464 V[i17 + 3 * k] += b_A4[i17 + 3 * i18] * A2[i18 + 3 * k];
00465 }
00466 }
00467 }
00468
00469 for (i17 = 0; i17 < 9; i17++) {
00470 U[i17] = ((V[i17] + 3960.0 * b_A4[i17]) + 2.16216E+6 * A3[i17]) +
00471 3.027024E+8 * A2[i17];
00472 }
00473
00474 for (k = 0; k < 3; k++) {
00475 U[k + 3 * k] += 8.8216128E+9;
00476 }
00477
00478 for (i17 = 0; i17 < 3; i17++) {
00479 for (k = 0; k < 3; k++) {
00480 A4[i17 + 3 * k] = 0.0;
00481 for (i18 = 0; i18 < 3; i18++) {
00482 A4[i17 + 3 * k] += A[i17 + 3 * i18] * U[i18 + 3 * k];
00483 }
00484 }
00485 }
00486
00487 for (i17 = 0; i17 < 3; i17++) {
00488 for (k = 0; k < 3; k++) {
00489 U[k + 3 * i17] = A4[k + 3 * i17];
00490 }
00491 }
00492
00493 for (i17 = 0; i17 < 9; i17++) {
00494 V[i17] = ((90.0 * V[i17] + 110880.0 * b_A4[i17]) + 3.027024E+7 *
00495 A3[i17]) + 2.0756736E+9 * A2[i17];
00496 }
00497
00498 d = 1.76432256E+10;
00499 } else {
00500 for (i17 = 0; i17 < 9; i17++) {
00501 U[i17] = (3.352212864E+10 * b_A4[i17] + 1.05594705216E+13 * A3[i17]) +
00502 1.1873537964288E+15 * A2[i17];
00503 }
00504
00505 for (k = 0; k < 3; k++) {
00506 U[k + 3 * k] += 3.238237626624E+16;
00507 for (i17 = 0; i17 < 3; i17++) {
00508 A4[i17 + 3 * k] = (b_A4[i17 + 3 * k] + 16380.0 * A3[i17 + 3 * k]) +
00509 4.08408E+7 * A2[i17 + 3 * k];
00510 }
00511 }
00512
00513 for (i17 = 0; i17 < 3; i17++) {
00514 for (k = 0; k < 3; k++) {
00515 d = 0.0;
00516 for (i18 = 0; i18 < 3; i18++) {
00517 d += b_A4[i17 + 3 * i18] * A4[i18 + 3 * k];
00518 }
00519
00520 V[i17 + 3 * k] = d + U[i17 + 3 * k];
00521 }
00522 }
00523
00524 for (i17 = 0; i17 < 3; i17++) {
00525 for (k = 0; k < 3; k++) {
00526 U[i17 + 3 * k] = 0.0;
00527 for (i18 = 0; i18 < 3; i18++) {
00528 U[i17 + 3 * k] += A[i17 + 3 * i18] * V[i18 + 3 * k];
00529 }
00530
00531 A4[k + 3 * i17] = (182.0 * b_A4[k + 3 * i17] + 960960.0 * A3[k + 3 *
00532 i17]) + 1.32324192E+9 * A2[k + 3 * i17];
00533 }
00534 }
00535
00536 for (i17 = 0; i17 < 3; i17++) {
00537 for (k = 0; k < 3; k++) {
00538 d = 0.0;
00539 for (i18 = 0; i18 < 3; i18++) {
00540 d += b_A4[i17 + 3 * i18] * A4[i18 + 3 * k];
00541 }
00542
00543 V[i17 + 3 * k] = ((d + 6.704425728E+11 * b_A4[i17 + 3 * k]) +
00544 1.29060195264E+14 * A3[i17 + 3 * k]) +
00545 7.7717703038976E+15 * A2[i17 + 3 * k];
00546 }
00547 }
00548
00549 d = 6.476475253248E+16;
00550 }
00551 }
00552 }
00553
00554 for (k = 0; k < 3; k++) {
00555 V[k + 3 * k] += d;
00556 }
00557
00558 for (k = 0; k < 9; k++) {
00559 d = V[k] - U[k];
00560 V[k] += U[k];
00561 U[k] = d;
00562 }
00563
00564 b_mldivide(U, V, F);
00565 }
00566
00567 static void c_PadeApproximantOfDegree(const real_T A[4], real_T m, real_T F[4])
00568 {
00569 int32_T i22;
00570 int32_T k;
00571 real_T A2[4];
00572 int32_T i23;
00573 real_T U[4];
00574 real_T A4[4];
00575 real_T V[4];
00576 real_T d;
00577 real_T A3[4];
00578 real_T b_A4[4];
00579 for (i22 = 0; i22 < 2; i22++) {
00580 for (k = 0; k < 2; k++) {
00581 A2[i22 + (k << 1)] = 0.0;
00582 for (i23 = 0; i23 < 2; i23++) {
00583 A2[i22 + (k << 1)] += A[i22 + (i23 << 1)] * A[i23 + (k << 1)];
00584 }
00585 }
00586 }
00587
00588 if (m == 3.0) {
00589 for (i22 = 0; i22 < 4; i22++) {
00590 U[i22] = A2[i22];
00591 }
00592
00593 for (k = 0; k < 2; k++) {
00594 U[k + (k << 1)] += 60.0;
00595 }
00596
00597 for (i22 = 0; i22 < 2; i22++) {
00598 for (k = 0; k < 2; k++) {
00599 A4[i22 + (k << 1)] = 0.0;
00600 for (i23 = 0; i23 < 2; i23++) {
00601 A4[i22 + (k << 1)] += A[i22 + (i23 << 1)] * U[i23 + (k << 1)];
00602 }
00603 }
00604 }
00605
00606 for (i22 = 0; i22 < 2; i22++) {
00607 for (k = 0; k < 2; k++) {
00608 U[k + (i22 << 1)] = A4[k + (i22 << 1)];
00609 }
00610 }
00611
00612 for (i22 = 0; i22 < 4; i22++) {
00613 V[i22] = 12.0 * A2[i22];
00614 }
00615
00616 d = 120.0;
00617 } else {
00618 for (i22 = 0; i22 < 2; i22++) {
00619 for (k = 0; k < 2; k++) {
00620 A3[i22 + (k << 1)] = 0.0;
00621 for (i23 = 0; i23 < 2; i23++) {
00622 A3[i22 + (k << 1)] += A2[i22 + (i23 << 1)] * A2[i23 + (k << 1)];
00623 }
00624 }
00625 }
00626
00627 if (m == 5.0) {
00628 for (i22 = 0; i22 < 4; i22++) {
00629 U[i22] = A3[i22] + 420.0 * A2[i22];
00630 }
00631
00632 for (k = 0; k < 2; k++) {
00633 U[k + (k << 1)] += 15120.0;
00634 }
00635
00636 for (i22 = 0; i22 < 2; i22++) {
00637 for (k = 0; k < 2; k++) {
00638 A4[i22 + (k << 1)] = 0.0;
00639 for (i23 = 0; i23 < 2; i23++) {
00640 A4[i22 + (k << 1)] += A[i22 + (i23 << 1)] * U[i23 + (k << 1)];
00641 }
00642 }
00643 }
00644
00645 for (i22 = 0; i22 < 2; i22++) {
00646 for (k = 0; k < 2; k++) {
00647 U[k + (i22 << 1)] = A4[k + (i22 << 1)];
00648 }
00649 }
00650
00651 for (i22 = 0; i22 < 4; i22++) {
00652 V[i22] = 30.0 * A3[i22] + 3360.0 * A2[i22];
00653 }
00654
00655 d = 30240.0;
00656 } else {
00657 for (i22 = 0; i22 < 2; i22++) {
00658 for (k = 0; k < 2; k++) {
00659 b_A4[i22 + (k << 1)] = 0.0;
00660 for (i23 = 0; i23 < 2; i23++) {
00661 b_A4[i22 + (k << 1)] += A3[i22 + (i23 << 1)] * A2[i23 + (k << 1)];
00662 }
00663 }
00664 }
00665
00666 if (m == 7.0) {
00667 for (i22 = 0; i22 < 4; i22++) {
00668 U[i22] = (b_A4[i22] + 1512.0 * A3[i22]) + 277200.0 * A2[i22];
00669 }
00670
00671 for (k = 0; k < 2; k++) {
00672 U[k + (k << 1)] += 8.64864E+6;
00673 }
00674
00675 for (i22 = 0; i22 < 2; i22++) {
00676 for (k = 0; k < 2; k++) {
00677 A4[i22 + (k << 1)] = 0.0;
00678 for (i23 = 0; i23 < 2; i23++) {
00679 A4[i22 + (k << 1)] += A[i22 + (i23 << 1)] * U[i23 + (k << 1)];
00680 }
00681 }
00682 }
00683
00684 for (i22 = 0; i22 < 2; i22++) {
00685 for (k = 0; k < 2; k++) {
00686 U[k + (i22 << 1)] = A4[k + (i22 << 1)];
00687 }
00688 }
00689
00690 for (i22 = 0; i22 < 4; i22++) {
00691 V[i22] = (56.0 * b_A4[i22] + 25200.0 * A3[i22]) + 1.99584E+6 * A2[i22];
00692 }
00693
00694 d = 1.729728E+7;
00695 } else if (m == 9.0) {
00696 for (i22 = 0; i22 < 2; i22++) {
00697 for (k = 0; k < 2; k++) {
00698 V[i22 + (k << 1)] = 0.0;
00699 for (i23 = 0; i23 < 2; i23++) {
00700 V[i22 + (k << 1)] += b_A4[i22 + (i23 << 1)] * A2[i23 + (k << 1)];
00701 }
00702 }
00703 }
00704
00705 for (i22 = 0; i22 < 4; i22++) {
00706 U[i22] = ((V[i22] + 3960.0 * b_A4[i22]) + 2.16216E+6 * A3[i22]) +
00707 3.027024E+8 * A2[i22];
00708 }
00709
00710 for (k = 0; k < 2; k++) {
00711 U[k + (k << 1)] += 8.8216128E+9;
00712 }
00713
00714 for (i22 = 0; i22 < 2; i22++) {
00715 for (k = 0; k < 2; k++) {
00716 A4[i22 + (k << 1)] = 0.0;
00717 for (i23 = 0; i23 < 2; i23++) {
00718 A4[i22 + (k << 1)] += A[i22 + (i23 << 1)] * U[i23 + (k << 1)];
00719 }
00720 }
00721 }
00722
00723 for (i22 = 0; i22 < 2; i22++) {
00724 for (k = 0; k < 2; k++) {
00725 U[k + (i22 << 1)] = A4[k + (i22 << 1)];
00726 }
00727 }
00728
00729 for (i22 = 0; i22 < 4; i22++) {
00730 V[i22] = ((90.0 * V[i22] + 110880.0 * b_A4[i22]) + 3.027024E+7 *
00731 A3[i22]) + 2.0756736E+9 * A2[i22];
00732 }
00733
00734 d = 1.76432256E+10;
00735 } else {
00736 for (i22 = 0; i22 < 4; i22++) {
00737 U[i22] = (3.352212864E+10 * b_A4[i22] + 1.05594705216E+13 * A3[i22]) +
00738 1.1873537964288E+15 * A2[i22];
00739 }
00740
00741 for (k = 0; k < 2; k++) {
00742 U[k + (k << 1)] += 3.238237626624E+16;
00743 for (i22 = 0; i22 < 2; i22++) {
00744 A4[i22 + (k << 1)] = (b_A4[i22 + (k << 1)] + 16380.0 * A3[i22 + (k <<
00745 1)]) + 4.08408E+7 * A2[i22 + (k << 1)];
00746 }
00747 }
00748
00749 for (i22 = 0; i22 < 2; i22++) {
00750 for (k = 0; k < 2; k++) {
00751 d = 0.0;
00752 for (i23 = 0; i23 < 2; i23++) {
00753 d += b_A4[i22 + (i23 << 1)] * A4[i23 + (k << 1)];
00754 }
00755
00756 V[i22 + (k << 1)] = d + U[i22 + (k << 1)];
00757 }
00758 }
00759
00760 for (i22 = 0; i22 < 2; i22++) {
00761 for (k = 0; k < 2; k++) {
00762 U[i22 + (k << 1)] = 0.0;
00763 for (i23 = 0; i23 < 2; i23++) {
00764 U[i22 + (k << 1)] += A[i22 + (i23 << 1)] * V[i23 + (k << 1)];
00765 }
00766
00767 A4[k + (i22 << 1)] = (182.0 * b_A4[k + (i22 << 1)] + 960960.0 * A3[k
00768 + (i22 << 1)]) + 1.32324192E+9 * A2[k + (i22 <<
00769 1)];
00770 }
00771 }
00772
00773 for (i22 = 0; i22 < 2; i22++) {
00774 for (k = 0; k < 2; k++) {
00775 d = 0.0;
00776 for (i23 = 0; i23 < 2; i23++) {
00777 d += b_A4[i22 + (i23 << 1)] * A4[i23 + (k << 1)];
00778 }
00779
00780 V[i22 + (k << 1)] = ((d + 6.704425728E+11 * b_A4[i22 + (k << 1)]) +
00781 1.29060195264E+14 * A3[i22 + (k << 1)]) +
00782 7.7717703038976E+15 * A2[i22 + (k << 1)];
00783 }
00784 }
00785
00786 d = 6.476475253248E+16;
00787 }
00788 }
00789 }
00790
00791 for (k = 0; k < 2; k++) {
00792 V[k + (k << 1)] += d;
00793 }
00794
00795 for (k = 0; k < 4; k++) {
00796 d = V[k] - U[k];
00797 V[k] += U[k];
00798 U[k] = d;
00799 }
00800
00801 c_mldivide(U, V, F);
00802 }
00803
00804 static real_T rt_powd_snf(real_T u0, real_T u1)
00805 {
00806 real_T y;
00807 real_T d4;
00808 real_T d5;
00809 if (rtIsNaN(u0) || rtIsNaN(u1)) {
00810 y = rtNaN;
00811 } else {
00812 d4 = fabs(u0);
00813 d5 = fabs(u1);
00814 if (rtIsInf(u1)) {
00815 if (d4 == 1.0) {
00816 y = rtNaN;
00817 } else if (d4 > 1.0) {
00818 if (u1 > 0.0) {
00819 y = rtInf;
00820 } else {
00821 y = 0.0;
00822 }
00823 } else if (u1 > 0.0) {
00824 y = 0.0;
00825 } else {
00826 y = rtInf;
00827 }
00828 } else if (d5 == 0.0) {
00829 y = 1.0;
00830 } else if (d5 == 1.0) {
00831 if (u1 > 0.0) {
00832 y = u0;
00833 } else {
00834 y = 1.0 / u0;
00835 }
00836 } else if (u1 == 2.0) {
00837 y = u0 * u0;
00838 } else if ((u1 == 0.5) && (u0 >= 0.0)) {
00839 y = sqrt(u0);
00840 } else if ((u0 < 0.0) && (u1 > floor(u1))) {
00841 y = rtNaN;
00842 } else {
00843 y = pow(u0, u1);
00844 }
00845 }
00846
00847 return y;
00848 }
00849
00850 void b_expm(real_T A[9], real_T F[9])
00851 {
00852 real_T normA;
00853 int32_T j;
00854 boolean_T exitg2;
00855 real_T s;
00856 int32_T i;
00857 boolean_T exitg1;
00858 static const real_T theta[5] = { 0.01495585217958292, 0.253939833006323,
00859 0.95041789961629319, 2.097847961257068, 5.3719203511481517 };
00860
00861 static const int8_T iv7[5] = { 3, 5, 7, 9, 13 };
00862
00863 int32_T eint;
00864 real_T b_F[9];
00865 int32_T i16;
00866 normA = 0.0;
00867 j = 0;
00868 exitg2 = FALSE;
00869 while ((exitg2 == 0U) && (j < 3)) {
00870 s = 0.0;
00871 for (i = 0; i < 3; i++) {
00872 s += fabs(A[i + 3 * j]);
00873 }
00874
00875 if (rtIsNaN(s)) {
00876 normA = rtNaN;
00877 exitg2 = TRUE;
00878 } else {
00879 if (s > normA) {
00880 normA = s;
00881 }
00882
00883 j++;
00884 }
00885 }
00886
00887 if (normA <= 5.3719203511481517) {
00888 i = 0;
00889 exitg1 = FALSE;
00890 while ((exitg1 == 0U) && (i < 5)) {
00891 if (normA <= theta[i]) {
00892 b_PadeApproximantOfDegree(A, (real_T)iv7[i], F);
00893 exitg1 = TRUE;
00894 } else {
00895 i++;
00896 }
00897 }
00898 } else {
00899 normA /= 5.3719203511481517;
00900 if ((!rtIsInf(normA)) && (!rtIsNaN(normA))) {
00901 normA = frexp(normA, &eint);
00902 } else {
00903 eint = 0;
00904 }
00905
00906 s = (real_T)eint;
00907 if (normA == 0.5) {
00908 s = (real_T)eint - 1.0;
00909 }
00910
00911 normA = rt_powd_snf(2.0, s);
00912 for (i = 0; i < 9; i++) {
00913 A[i] /= normA;
00914 }
00915
00916 b_PadeApproximantOfDegree(A, 13.0, F);
00917 for (j = 0; j <= (int32_T)s - 1; j++) {
00918 for (i = 0; i < 3; i++) {
00919 for (eint = 0; eint < 3; eint++) {
00920 b_F[i + 3 * eint] = 0.0;
00921 for (i16 = 0; i16 < 3; i16++) {
00922 b_F[i + 3 * eint] += F[i + 3 * i16] * F[i16 + 3 * eint];
00923 }
00924 }
00925 }
00926
00927 for (i = 0; i < 3; i++) {
00928 for (eint = 0; eint < 3; eint++) {
00929 F[eint + 3 * i] = b_F[eint + 3 * i];
00930 }
00931 }
00932 }
00933 }
00934 }
00935
00936 void c_expm(real_T A[4], real_T F[4])
00937 {
00938 real_T normA;
00939 int32_T j;
00940 boolean_T exitg2;
00941 real_T s;
00942 int32_T i;
00943 boolean_T exitg1;
00944 static const real_T theta[5] = { 0.01495585217958292, 0.253939833006323,
00945 0.95041789961629319, 2.097847961257068, 5.3719203511481517 };
00946
00947 static const int8_T iv9[5] = { 3, 5, 7, 9, 13 };
00948
00949 int32_T eint;
00950 real_T b_F[4];
00951 int32_T i21;
00952 normA = 0.0;
00953 j = 0;
00954 exitg2 = FALSE;
00955 while ((exitg2 == 0U) && (j < 2)) {
00956 s = 0.0;
00957 for (i = 0; i < 2; i++) {
00958 s += fabs(A[i + (j << 1)]);
00959 }
00960
00961 if (rtIsNaN(s)) {
00962 normA = rtNaN;
00963 exitg2 = TRUE;
00964 } else {
00965 if (s > normA) {
00966 normA = s;
00967 }
00968
00969 j++;
00970 }
00971 }
00972
00973 if (normA <= 5.3719203511481517) {
00974 i = 0;
00975 exitg1 = FALSE;
00976 while ((exitg1 == 0U) && (i < 5)) {
00977 if (normA <= theta[i]) {
00978 c_PadeApproximantOfDegree(A, (real_T)iv9[i], F);
00979 exitg1 = TRUE;
00980 } else {
00981 i++;
00982 }
00983 }
00984 } else {
00985 normA /= 5.3719203511481517;
00986 if ((!rtIsInf(normA)) && (!rtIsNaN(normA))) {
00987 normA = frexp(normA, &eint);
00988 } else {
00989 eint = 0;
00990 }
00991
00992 s = (real_T)eint;
00993 if (normA == 0.5) {
00994 s = (real_T)eint - 1.0;
00995 }
00996
00997 normA = rt_powd_snf(2.0, s);
00998 for (i = 0; i < 4; i++) {
00999 A[i] /= normA;
01000 }
01001
01002 c_PadeApproximantOfDegree(A, 13.0, F);
01003 for (j = 0; j <= (int32_T)s - 1; j++) {
01004 for (i = 0; i < 2; i++) {
01005 for (eint = 0; eint < 2; eint++) {
01006 b_F[i + (eint << 1)] = 0.0;
01007 for (i21 = 0; i21 < 2; i21++) {
01008 b_F[i + (eint << 1)] += F[i + (i21 << 1)] * F[i21 + (eint << 1)];
01009 }
01010 }
01011 }
01012
01013 for (i = 0; i < 2; i++) {
01014 for (eint = 0; eint < 2; eint++) {
01015 F[eint + (i << 1)] = b_F[eint + (i << 1)];
01016 }
01017 }
01018 }
01019 }
01020 }
01021
01022 void expm(creal_T A[9], creal_T F[9])
01023 {
01024 real_T normA;
01025 int32_T j;
01026 boolean_T exitg2;
01027 real_T s;
01028 int32_T i;
01029 boolean_T exitg1;
01030 static const real_T theta[5] = { 0.01495585217958292, 0.253939833006323,
01031 0.95041789961629319, 2.097847961257068, 5.3719203511481517 };
01032
01033 static const int8_T iv5[5] = { 3, 5, 7, 9, 13 };
01034
01035 int32_T eint;
01036 real_T A_im;
01037 creal_T b_F[9];
01038 int32_T i7;
01039 normA = 0.0;
01040 j = 0;
01041 exitg2 = FALSE;
01042 while ((exitg2 == 0U) && (j < 3)) {
01043 s = 0.0;
01044 for (i = 0; i < 3; i++) {
01045 s += rt_hypotd_snf(fabs(A[i + 3 * j].re), fabs(A[i + 3 * j].im));
01046 }
01047
01048 if (rtIsNaN(s)) {
01049 normA = rtNaN;
01050 exitg2 = TRUE;
01051 } else {
01052 if (s > normA) {
01053 normA = s;
01054 }
01055
01056 j++;
01057 }
01058 }
01059
01060 if (normA <= 5.3719203511481517) {
01061 i = 0;
01062 exitg1 = FALSE;
01063 while ((exitg1 == 0U) && (i < 5)) {
01064 if (normA <= theta[i]) {
01065 PadeApproximantOfDegree(A, (real_T)iv5[i], F);
01066 exitg1 = TRUE;
01067 } else {
01068 i++;
01069 }
01070 }
01071 } else {
01072 normA /= 5.3719203511481517;
01073 if ((!rtIsInf(normA)) && (!rtIsNaN(normA))) {
01074 normA = frexp(normA, &eint);
01075 } else {
01076 eint = 0;
01077 }
01078
01079 s = (real_T)eint;
01080 if (normA == 0.5) {
01081 s = (real_T)eint - 1.0;
01082 }
01083
01084 normA = rt_powd_snf(2.0, s);
01085 for (i = 0; i < 9; i++) {
01086 A_im = A[i].im;
01087 if (A[i].im == 0.0) {
01088 A[i].re /= normA;
01089 A[i].im = 0.0;
01090 } else if (A[i].re == 0.0) {
01091 A[i].re = 0.0;
01092 A[i].im = A_im / normA;
01093 } else {
01094 A[i].re /= normA;
01095 A[i].im = A_im / normA;
01096 }
01097 }
01098
01099 PadeApproximantOfDegree(A, 13.0, F);
01100 for (j = 0; j <= (int32_T)s - 1; j++) {
01101 for (i = 0; i < 3; i++) {
01102 for (eint = 0; eint < 3; eint++) {
01103 b_F[i + 3 * eint].re = 0.0;
01104 b_F[i + 3 * eint].im = 0.0;
01105 for (i7 = 0; i7 < 3; i7++) {
01106 b_F[i + 3 * eint].re += F[i + 3 * i7].re * F[i7 + 3 * eint].re - F[i
01107 + 3 * i7].im * F[i7 + 3 * eint].im;
01108 b_F[i + 3 * eint].im += F[i + 3 * i7].re * F[i7 + 3 * eint].im + F[i
01109 + 3 * i7].im * F[i7 + 3 * eint].re;
01110 }
01111 }
01112 }
01113
01114 for (i = 0; i < 3; i++) {
01115 for (eint = 0; eint < 3; eint++) {
01116 F[eint + 3 * i] = b_F[eint + 3 * i];
01117 }
01118 }
01119 }
01120 }
01121 }
01122
01123