00001
00002 #include <iostream>
00003 #include <vecmath/Matrix3SVD.h>
00004
00005 const double Matrix3SVD::EPS1 = 1.110223024E-16;
00006 const double Matrix3SVD::EPS2 = 4.89E-15;
00007 const double Matrix3SVD::EPS3 = 1E-08;
00008
00009
00010
00011
00012 bool Matrix3SVD::almostEqual(double d, double d1)
00013 {
00014 if (d == d1)
00015 return true;
00016 const double d2 = 1E-06;
00017
00018 double d4 = Math::abs(d - d1);
00019 double d5 = Math::abs(d);
00020 double d6 = Math::abs(d1);
00021 double d7 = d5 < d6 ? d6 : d5;
00022 if (d4 < d2)
00023 return true;
00024 return d4 / d7 < 0.0001;
00025 }
00026
00027 int Matrix3SVD::compute_2X2(double d, double d1, double d2, double ad[], double ad1[],
00028 double ad2[], double ad3[], double ad4[], int i)
00029 {
00030 double d3 = 2.0;
00031 double d4 = 1.0;
00032 double d37 = ad[0];
00033 double d36 = ad[1];
00034 double d32 = 0.0;
00035 double d33 = 0.0;
00036 double d34 = 0.0;
00037 double d35 = 0.0;
00038 double d21 = 0.0;
00039 double d25 = d;
00040 double d22 = Math::abs(d25);
00041 double d27 = d2;
00042 double d24 = Math::abs(d2);
00043 unsigned char byte0 = 1;
00044 bool flag;
00045 if (d24 > d22)
00046 flag = true;
00047 else
00048 flag = false;
00049 if (flag) {
00050 byte0 = 3;
00051 double d6 = d25;
00052 d25 = d27;
00053 d27 = d6;
00054 d6 = d22;
00055 d22 = d24;
00056 d24 = d6;
00057 }
00058 double d26 = d1;
00059 double d23 = Math::abs(d26);
00060 if (d23 == 0.0) {
00061 ad[1] = d24;
00062 ad[0] = d22;
00063 d32 = 1.0;
00064 d33 = 1.0;
00065 d34 = 0.0;
00066 d35 = 0.0;
00067 } else {
00068 bool flag1 = true;
00069 if (d23 > d22) {
00070 byte0 = 2;
00071 if (d22 / d23 < EPS1) {
00072 flag1 = false;
00073 d37 = d23;
00074 if (d24 > 1.0)
00075 d36 = d22 / (d23 / d24);
00076 else
00077 d36 = (d22 / d23) * d24;
00078 d32 = 1.0;
00079 d34 = d27 / d26;
00080 d35 = 1.0;
00081 d33 = d25 / d26;
00082 }
00083 }
00084 if (flag1) {
00085 double d9 = d22 - d24;
00086 double d11;
00087 if (d9 == d22)
00088 d11 = 1.0;
00089 else
00090 d11 = d9 / d22;
00091 double d13 = d26 / d25;
00092
00093 double d28 = d13 * d13;
00094
00095
00096 double d15;
00097 if (d11 == 0.0)
00098 d15 = Math::abs(d13);
00099 else
00100 d15 = sqrt(d11 * d11 + d28);
00101
00102 if (d23 > d22) {
00103 byte0 = 2;
00104 if (d22 / d23 < EPS1) {
00105 flag1 = false;
00106 d37 = d23;
00107 if (d24 > 1.0)
00108 d36 = d22 / (d23 / d24);
00109 else
00110 d36 = (d22 / d23) * d24;
00111 d32 = 1.0;
00112 d34 = d27 / d26;
00113 d35 = 1.0;
00114 d33 = d25 / d26;
00115 }
00116 }
00117 if (flag1) {
00118 double d10 = d22 - d24;
00119 double d12;
00120 if (d10 == d22)
00121 d12 = 1.0;
00122 else
00123 d12 = d10 / d22;
00124 double d14 = d26 / d25;
00125 double d20 = 2.0 - d12;
00126 double d29 = d14 * d14;
00127 double d31 = d20 * d20;
00128 double d18 = sqrt(d31 + d29);
00129 double d16;
00130 if (d12 == 0.0)
00131 d16 = Math::abs(d14);
00132 else
00133 d16 = sqrt(d12 * d12 + d29);
00134 double d8 = (d18 + d16) * 0.5;
00135 d36 = d24 / d8;
00136 d37 = d22 * d8;
00137 if (d29 == 0.0) {
00138 if (d12 == 0.0)
00139 d20 = d_sign(d3, d25) * d_sign(d4, d26);
00140 else
00141 d20 = d26 / d_sign(d10, d25) + d14 / d20;
00142 } else {
00143 d20 = (d14 / (d18 + d20) + d14 / (d16 + d12)) * (d8 + 1.0);
00144 }
00145 d12 = sqrt(d20 * d20 + 4);
00146 d33 = 2.0 / d12;
00147 d35 = d20 / d12;
00148 d32 = (d33 + d35 * d14) / d8;
00149 d34 = ((d27 / d25) * d35) / d8;
00150 }
00151 }
00152 if (flag) {
00153 ad2[0] = d35;
00154 ad1[0] = d33;
00155 ad4[0] = d34;
00156 ad3[0] = d32;
00157 } else {
00158 ad2[0] = d32;
00159 ad1[0] = d34;
00160 ad4[0] = d33;
00161 ad3[0] = d35;
00162 }
00163 if (byte0 == 1)
00164 d21 = d_sign(d4, ad4[0]) * d_sign(d4, ad2[0]) * d_sign(d4, d);
00165 if (byte0 == 2)
00166 d21 = d_sign(d4, ad3[0]) * d_sign(d4, ad2[0]) * d_sign(d4, d1);
00167 if (byte0 == 3)
00168 d21 = d_sign(d4, ad3[0]) * d_sign(d4, ad1[0]) * d_sign(d4, d2);
00169 ad[i] = d_sign(d37, d21);
00170 double d5 = d21 * d_sign(d4, d) * d_sign(d4, d2);
00171 ad[i + 1] = d_sign(d36, d5);
00172 }
00173 return 0;
00174 }
00175
00176 int Matrix3SVD::compute_qr(double ad[], double ad1[], double ad2[], double ad3[], bool flag)
00177 {
00178 double ad4[2];
00179 double ad5[2];
00180 double ad6[2];
00181 double ad7[2];
00182 double ad8[9];
00183 double ad9[9];
00184 double ad10[9];
00185 double ad11[9];
00186
00187 double d11 = 1.0;
00188 bool flag1 = false;
00189 if (flag) {
00190 std::cout << " \nu = \n" << std::endl;
00191 print_mat(ad2);
00192 std::cout << " \nv = \n" << std::endl;
00193 print_mat(ad3);
00194 ad8[0] = ad[0];
00195 ad8[1] = ad1[0];
00196 ad8[2] = 0.0;
00197 ad8[3] = 0.0;
00198 ad8[4] = ad[1];
00199 ad8[5] = ad1[1];
00200 ad8[6] = 0.0;
00201 ad8[7] = 0.0;
00202 ad8[8] = ad[2];
00203 transpose_mat(ad2, ad9);
00204 mat_mul(ad9, ad8, ad11);
00205 transpose_mat(ad3, ad10);
00206 mat_mul(ad11, ad10, ad11);
00207 std::cout << "\nA=\n" << std::endl;
00208 print_mat(ad11);
00209 }
00210 int j = 1;
00211 if (Math::abs(ad1[1]) < EPS2 || Math::abs(ad1[0]) < EPS2)
00212 flag1 = true;
00213 for (int i = 0; i < 10 && !flag1; i++) {
00214 double d = compute_shift(ad[1], ad1[1], ad[2]);
00215 double d8 = (Math::abs(ad[0]) - d) * (d_sign(d11, ad[0]) + d / ad[0]);
00216 double d9 = ad1[0];
00217 double d1 = compute_rot(d8, d9, ad7, ad5, 0, j);
00218 d8 = ad5[0] * ad[0] + ad7[0] * ad1[0];
00219 ad1[0] = ad5[0] * ad1[0] - ad7[0] * ad[0];
00220 d9 = ad7[0] * ad[1];
00221 ad[1] = ad5[0] * ad[1];
00222 d1 = compute_rot(d8, d9, ad6, ad4, 0, j);
00223 j = 0;
00224 ad[0] = d1;
00225 d8 = ad4[0] * ad1[0] + ad6[0] * ad[1];
00226 ad[1] = ad4[0] * ad[1] - ad6[0] * ad1[0];
00227 d9 = ad6[0] * ad1[1];
00228 ad1[1] = ad4[0] * ad1[1];
00229 d1 = compute_rot(d8, d9, ad7, ad5, 1, j);
00230 ad1[0] = d1;
00231 d8 = ad5[1] * ad[1] + ad7[1] * ad1[1];
00232 ad1[1] = ad5[1] * ad1[1] - ad7[1] * ad[1];
00233 d9 = ad7[1] * ad[2];
00234 ad[2] = ad5[1] * ad[2];
00235 d1 = compute_rot(d8, d9, ad6, ad4, 1, j);
00236 ad[1] = d1;
00237 d8 = ad4[1] * ad1[1] + ad6[1] * ad[2];
00238 ad[2] = ad4[1] * ad[2] - ad6[1] * ad1[1];
00239 ad1[1] = d8;
00240
00241 double d2 = ad2[0];
00242 ad2[0] = ad4[0] * d2 + ad6[0] * ad2[3];
00243 ad2[3] = -ad6[0] * d2 + ad4[0] * ad2[3];
00244 d2 = ad2[1];
00245 ad2[1] = ad4[0] * d2 + ad6[0] * ad2[4];
00246 ad2[4] = -ad6[0] * d2 + ad4[0] * ad2[4];
00247 d2 = ad2[2];
00248 ad2[2] = ad4[0] * d2 + ad6[0] * ad2[5];
00249 ad2[5] = -ad6[0] * d2 + ad4[0] * ad2[5];
00250 d2 = ad2[3];
00251 ad2[3] = ad4[1] * d2 + ad6[1] * ad2[6];
00252 ad2[6] = -ad6[1] * d2 + ad4[1] * ad2[6];
00253 d2 = ad2[4];
00254 ad2[4] = ad4[1] * d2 + ad6[1] * ad2[7];
00255 ad2[7] = -ad6[1] * d2 + ad4[1] * ad2[7];
00256 d2 = ad2[5];
00257 ad2[5] = ad4[1] * d2 + ad6[1] * ad2[8];
00258 ad2[8] = -ad6[1] * d2 + ad4[1] * ad2[8];
00259
00260 double d5 = ad3[0];
00261 ad3[0] = ad5[0] * d5 + ad7[0] * ad3[1];
00262 ad3[1] = -ad7[0] * d5 + ad5[0] * ad3[1];
00263 d5 = ad3[3];
00264 ad3[3] = ad5[0] * d5 + ad7[0] * ad3[4];
00265 ad3[4] = -ad7[0] * d5 + ad5[0] * ad3[4];
00266 d5 = ad3[6];
00267 ad3[6] = ad5[0] * d5 + ad7[0] * ad3[7];
00268 ad3[7] = -ad7[0] * d5 + ad5[0] * ad3[7];
00269 d5 = ad3[1];
00270 ad3[1] = ad5[1] * d5 + ad7[1] * ad3[2];
00271 ad3[2] = -ad7[1] * d5 + ad5[1] * ad3[2];
00272 d5 = ad3[4];
00273 ad3[4] = ad5[1] * d5 + ad7[1] * ad3[5];
00274 ad3[5] = -ad7[1] * d5 + ad5[1] * ad3[5];
00275 d5 = ad3[7];
00276 ad3[7] = ad5[1] * d5 + ad7[1] * ad3[8];
00277 ad3[8] = -ad7[1] * d5 + ad5[1] * ad3[8];
00278 if (flag)
00279 std::cout << "\n*********************** iteration #" << i << " ***********************\n" << std::endl;
00280 ad8[0] = ad[0];
00281 ad8[1] = ad1[0];
00282 ad8[2] = 0.0;
00283 ad8[3] = 0.0;
00284 ad8[4] = ad[1];
00285 ad8[5] = ad1[1];
00286 ad8[6] = 0.0;
00287 ad8[7] = 0.0;
00288 ad8[8] = ad[2];
00289 if (Math::abs(ad1[1]) < EPS2 || Math::abs(ad1[0]) < EPS2)
00290 flag1 = true;
00291 }
00292
00293 if (Math::abs(ad1[1]) < EPS2) {
00294 compute_2X2(ad[0], ad1[0], ad[1], ad, ad6, ad4, ad7, ad5, 0);
00295 double d3 = ad2[0];
00296 ad2[0] = ad4[0] * d3 + ad6[0] * ad2[3];
00297 ad2[3] = -ad6[0] * d3 + ad4[0] * ad2[3];
00298 d3 = ad2[1];
00299 ad2[1] = ad4[0] * d3 + ad6[0] * ad2[4];
00300 ad2[4] = -ad6[0] * d3 + ad4[0] * ad2[4];
00301 d3 = ad2[2];
00302 ad2[2] = ad4[0] * d3 + ad6[0] * ad2[5];
00303 ad2[5] = -ad6[0] * d3 + ad4[0] * ad2[5];
00304 double d6 = ad3[0];
00305 ad3[0] = ad5[0] * d6 + ad7[0] * ad3[1];
00306 ad3[1] = -ad7[0] * d6 + ad5[0] * ad3[1];
00307 d6 = ad3[3];
00308 ad3[3] = ad5[0] * d6 + ad7[0] * ad3[4];
00309 ad3[4] = -ad7[0] * d6 + ad5[0] * ad3[4];
00310 d6 = ad3[6];
00311 ad3[6] = ad5[0] * d6 + ad7[0] * ad3[7];
00312 ad3[7] = -ad7[0] * d6 + ad5[0] * ad3[7];
00313 } else {
00314 compute_2X2(ad[1], ad1[1], ad[2], ad, ad6, ad4, ad7, ad5, 1);
00315 double d4 = ad2[3];
00316 ad2[3] = ad4[0] * d4 + ad6[0] * ad2[6];
00317 ad2[6] = -ad6[0] * d4 + ad4[0] * ad2[6];
00318 d4 = ad2[4];
00319 ad2[4] = ad4[0] * d4 + ad6[0] * ad2[7];
00320 ad2[7] = -ad6[0] * d4 + ad4[0] * ad2[7];
00321 d4 = ad2[5];
00322 ad2[5] = ad4[0] * d4 + ad6[0] * ad2[8];
00323 ad2[8] = -ad6[0] * d4 + ad4[0] * ad2[8];
00324 double d7 = ad3[1];
00325 ad3[1] = ad5[0] * d7 + ad7[0] * ad3[2];
00326 ad3[2] = -ad7[0] * d7 + ad5[0] * ad3[2];
00327 d7 = ad3[4];
00328 ad3[4] = ad5[0] * d7 + ad7[0] * ad3[5];
00329 ad3[5] = -ad7[0] * d7 + ad5[0] * ad3[5];
00330 d7 = ad3[7];
00331 ad3[7] = ad5[0] * d7 + ad7[0] * ad3[8];
00332 ad3[8] = -ad7[0] * d7 + ad5[0] * ad3[8];
00333 }
00334 return 0;
00335 }
00336
00337 double Matrix3SVD::compute_rot(double d, double d1, double ad[], double ad1[], int i, int j)
00338 {
00339 const double d8 = 2.0020830951831009E-146;
00340 const double d9 = 4.9947976805055876E+145;
00341 double d2;
00342 double d3;
00343 double d7;
00344 if (d1 == 0.0) {
00345 d2 = 1.0;
00346 d3 = 0.0;
00347 d7 = d;
00348 } else
00349 if (d == 0.0) {
00350 d2 = 0.0;
00351 d3 = 1.0;
00352 d7 = d1;
00353 } else {
00354 double d5 = d;
00355 double d6 = d1;
00356 double d4 = max(Math::abs(d5), Math::abs(d6));
00357 if (d4 >= d9) {
00358 int k1 = 0;
00359 for (; d4 >= d9; d4 = max(Math::abs(d5), Math::abs(d6))) {
00360 k1++;
00361 d5 *= d8;
00362 d6 *= d8;
00363 }
00364
00365 d7 = sqrt(d5 * d5 + d6 * d6);
00366 d2 = d5 / d7;
00367 d3 = d6 / d7;
00368 for (int i1 = 1; i1 <= k1; i1++)
00369 d7 *= d9;
00370
00371 } else
00372 if (d4 <= d8) {
00373 int l1 = 0;
00374 for (; d4 <= d8; d4 = max(Math::abs(d5), Math::abs(d6))) {
00375 l1++;
00376 d5 *= d9;
00377 d6 *= d9;
00378 }
00379
00380 d7 = sqrt(d5 * d5 + d6 * d6);
00381 d2 = d5 / d7;
00382 d3 = d6 / d7;
00383 for (int j1 = 1; j1 <= l1; j1++)
00384 d7 *= d8;
00385
00386 } else {
00387 d7 = sqrt(d5 * d5 + d6 * d6);
00388 d2 = d5 / d7;
00389 d3 = d6 / d7;
00390 }
00391 if (Math::abs(d) > Math::abs(d1) && d2 < 0.0) {
00392 d2 = -d2;
00393 d3 = -d3;
00394 d7 = -d7;
00395 }
00396 }
00397 ad[i] = d3;
00398 ad1[i] = d2;
00399 return d7;
00400 }
00401
00402 double Matrix3SVD::compute_shift(double d, double d1, double d2)
00403 {
00404 double d11 = Math::abs(d);
00405 double d12 = Math::abs(d1);
00406 double d13 = Math::abs(d2);
00407 double d7 = min(d11, d13);
00408 double d8 = max(d11, d13);
00409 double d20;
00410 if (d7 == 0.0) {
00411 d20 = 0.0;
00412 double d3;
00413 if (d8 != 0.0)
00414 d3 = min(d8, d12) / max(d8, d12);
00415 } else
00416 if (d12 < d8) {
00417 double d14 = d7 / d8 + 1.0;
00418 double d16 = (d8 - d7) / d8;
00419 double d4 = d12 / d8;
00420 double d18 = d4 * d4;
00421 double d9 = 2.0 / (sqrt(d14 * d14 + d18) + sqrt(d16 * d16 + d18));
00422 d20 = d7 * d9;
00423 } else {
00424 double d19 = d8 / d12;
00425 if (d19 == 0.0) {
00426 d20 = (d7 * d8) / d12;
00427 } else {
00428 double d15 = d7 / d8 + 1.0;
00429 double d17 = (d8 - d7) / d8;
00430 double d5 = d15 * d19;
00431 double d6 = d17 * d19;
00432 double d10 = 1.0 / (sqrt(d5 * d5 + 1.0) + sqrt(d6 * d6 + 1.0));
00433 d20 = d7 * d10 * d19;
00434 d20 += d20;
00435 }
00436 }
00437 return d20;
00438 }
00439
00440 void Matrix3SVD::compute_svd(double ad[], double scale[], double rot[], bool flag)
00441 {
00442 double ad3[9];
00443 double ad4[9];
00444 double ad5[9];
00445 double ad6[9];
00446 double ad7[9];
00447 double ad8[9];
00448 double ad9[9];
00449 double ad10[3];
00450 double ad11[3];
00451 double ad12[3];
00452 int l = 0;
00453 if (flag) {
00454 std::cout << "input to compute_svd = \n" << std::endl;
00455 print_mat(ad);
00456 }
00457
00458 set(0.0, ad6);
00459
00460 copy(ad, ad9);
00461
00462 if (ad[3] * ad[3] < EPS1) {
00463 set(1.0, ad3);
00464 } else
00465 if (ad[0] * ad[0] < EPS1) {
00466 ad8[0] = ad[0];
00467 ad8[1] = ad[1];
00468 ad8[2] = ad[2];
00469 ad[0] = ad[3];
00470 ad[1] = ad[4];
00471 ad[2] = ad[5];
00472 ad[3] = -ad8[0];
00473 ad[4] = -ad8[1];
00474 ad[5] = -ad8[2];
00475 ad3[0] = 0.0;
00476 ad3[1] = 1.0;
00477 ad3[2] = 0.0;
00478 ad3[3] = -1;
00479 ad3[4] = 0.0;
00480 ad3[5] = 0.0;
00481 ad3[6] = 0.0;
00482 ad3[7] = 0.0;
00483 ad3[8] = 1.0;
00484 } else {
00485 double d = 1.0 / sqrt(ad[0] * ad[0] + ad[3] * ad[3]);
00486 double d4 = ad[0] * d;
00487 double d8 = ad[3] * d;
00488 ad8[0] = d4 * ad[0] + d8 * ad[3];
00489 ad8[1] = d4 * ad[1] + d8 * ad[4];
00490 ad8[2] = d4 * ad[2] + d8 * ad[5];
00491 ad[3] = -d8 * ad[0] + d4 * ad[3];
00492 ad[4] = -d8 * ad[1] + d4 * ad[4];
00493 ad[5] = -d8 * ad[2] + d4 * ad[5];
00494 ad[0] = ad8[0];
00495 ad[1] = ad8[1];
00496 ad[2] = ad8[2];
00497 ad3[0] = d4;
00498 ad3[1] = d8;
00499 ad3[2] = 0.0;
00500 ad3[3] = -d8;
00501 ad3[4] = d4;
00502 ad3[5] = 0.0;
00503 ad3[6] = 0.0;
00504 ad3[7] = 0.0;
00505 ad3[8] = 1.0;
00506 }
00507 if (ad[6] * ad[6] >= EPS1)
00508 if (ad[0] * ad[0] < EPS1) {
00509 ad8[0] = ad[0];
00510 ad8[1] = ad[1];
00511 ad8[2] = ad[2];
00512 ad[0] = ad[6];
00513 ad[1] = ad[7];
00514 ad[2] = ad[8];
00515 ad[6] = -ad8[0];
00516 ad[7] = -ad8[1];
00517 ad[8] = -ad8[2];
00518 ad8[0] = ad3[0];
00519 ad8[1] = ad3[1];
00520 ad8[2] = ad3[2];
00521 ad3[0] = ad3[6];
00522 ad3[1] = ad3[7];
00523 ad3[2] = ad3[8];
00524 ad3[6] = -ad8[0];
00525 ad3[7] = -ad8[1];
00526 ad3[8] = -ad8[2];
00527 } else {
00528 double d1 = 1.0 / sqrt(ad[0] * ad[0] + ad[6] * ad[6]);
00529 double d5 = ad[0] * d1;
00530 double d9 = ad[6] * d1;
00531 ad8[0] = d5 * ad[0] + d9 * ad[6];
00532 ad8[1] = d5 * ad[1] + d9 * ad[7];
00533 ad8[2] = d5 * ad[2] + d9 * ad[8];
00534 ad[6] = -d9 * ad[0] + d5 * ad[6];
00535 ad[7] = -d9 * ad[1] + d5 * ad[7];
00536 ad[8] = -d9 * ad[2] + d5 * ad[8];
00537 ad[0] = ad8[0];
00538 ad[1] = ad8[1];
00539 ad[2] = ad8[2];
00540 ad8[0] = d5 * ad3[0];
00541 ad8[1] = d5 * ad3[1];
00542 ad3[2] = d9;
00543 ad8[6] = -ad3[0] * d9;
00544 ad8[7] = -ad3[1] * d9;
00545 ad3[8] = d5;
00546 ad3[0] = ad8[0];
00547 ad3[1] = ad8[1];
00548 ad3[6] = ad8[6];
00549 ad3[7] = ad8[7];
00550 }
00551 if (ad[2] * ad[2] < EPS1) {
00552 set(1.0, ad4);
00553 } else
00554 if (ad[1] * ad[1] < EPS1) {
00555 ad8[2] = ad[2];
00556 ad8[5] = ad[5];
00557 ad8[8] = ad[8];
00558 ad[2] = -ad[1];
00559 ad[5] = -ad[4];
00560 ad[8] = -ad[7];
00561 ad[1] = ad8[2];
00562 ad[4] = ad8[5];
00563 ad[7] = ad8[8];
00564 ad4[0] = 1.0;
00565 ad4[1] = 0.0;
00566 ad4[2] = 0.0;
00567 ad4[3] = 0.0;
00568 ad4[4] = 0.0;
00569 ad4[5] = -1.0;
00570 ad4[6] = 0.0;
00571 ad4[7] = 1.0;
00572 ad4[8] = 0.0;
00573 } else {
00574 double d2 = 1.0 / sqrt(ad[1] * ad[1] + ad[2] * ad[2]);
00575 double d6 = ad[1] * d2;
00576 double d10 = ad[2] * d2;
00577 ad8[1] = d6 * ad[1] + d10 * ad[2];
00578 ad[2] = -d10 * ad[1] + d6 * ad[2];
00579 ad[1] = ad8[1];
00580 ad8[4] = d6 * ad[4] + d10 * ad[5];
00581 ad[5] = -d10 * ad[4] + d6 * ad[5];
00582 ad[4] = ad8[4];
00583 ad8[7] = d6 * ad[7] + d10 * ad[8];
00584 ad[8] = -d10 * ad[7] + d6 * ad[8];
00585 ad[7] = ad8[7];
00586 ad4[0] = 1.0;
00587 ad4[1] = 0.0;
00588 ad4[2] = 0.0;
00589 ad4[3] = 0.0;
00590 ad4[4] = d6;
00591 ad4[5] = -d10;
00592 ad4[6] = 0.0;
00593 ad4[7] = d10;
00594 ad4[8] = d6;
00595 }
00596 if (ad[7] * ad[7] >= EPS1)
00597 if (ad[4] * ad[4] < EPS1) {
00598 ad8[3] = ad[3];
00599 ad8[4] = ad[4];
00600 ad8[5] = ad[5];
00601 ad[3] = ad[6];
00602 ad[4] = ad[7];
00603 ad[5] = ad[8];
00604 ad[6] = -ad8[3];
00605 ad[7] = -ad8[4];
00606 ad[8] = -ad8[5];
00607 ad8[3] = ad3[3];
00608 ad8[4] = ad3[4];
00609 ad8[5] = ad3[5];
00610 ad3[3] = ad3[6];
00611 ad3[4] = ad3[7];
00612 ad3[5] = ad3[8];
00613 ad3[6] = -ad8[3];
00614 ad3[7] = -ad8[4];
00615 ad3[8] = -ad8[5];
00616 } else {
00617 double d3 = 1.0 / sqrt(ad[4] * ad[4] + ad[7] * ad[7]);
00618 double d7 = ad[4] * d3;
00619 double d11 = ad[7] * d3;
00620 ad8[3] = d7 * ad[3] + d11 * ad[6];
00621 ad[6] = -d11 * ad[3] + d7 * ad[6];
00622 ad[3] = ad8[3];
00623 ad8[4] = d7 * ad[4] + d11 * ad[7];
00624 ad[7] = -d11 * ad[4] + d7 * ad[7];
00625 ad[4] = ad8[4];
00626 ad8[5] = d7 * ad[5] + d11 * ad[8];
00627 ad[8] = -d11 * ad[5] + d7 * ad[8];
00628 ad[5] = ad8[5];
00629 ad8[3] = d7 * ad3[3] + d11 * ad3[6];
00630 ad3[6] = -d11 * ad3[3] + d7 * ad3[6];
00631 ad3[3] = ad8[3];
00632 ad8[4] = d7 * ad3[4] + d11 * ad3[7];
00633 ad3[7] = -d11 * ad3[4] + d7 * ad3[7];
00634 ad3[4] = ad8[4];
00635 ad8[5] = d7 * ad3[5] + d11 * ad3[8];
00636 ad3[8] = -d11 * ad3[5] + d7 * ad3[8];
00637 ad3[5] = ad8[5];
00638 }
00639 ad10[0] = ad[0];
00640 ad10[1] = ad[4];
00641 ad10[2] = ad[8];
00642 ad11[0] = ad[1];
00643 ad11[1] = ad[5];
00644 if (ad11[0] * ad11[0] < EPS1 && ad11[1] * ad11[1] < EPS1) {
00645 if (flag) {
00646 double xout, yout, zout;
00647 double xin, yin, zin;
00648 xin = yin = zin = 0;
00649 double d12 = 1.0;
00650 double d13 = 2.0;
00651 double d14 = 3.0;
00652 std::cout << "no QR iteration \n" << std::endl;
00653 ad7[0] = ad10[0];
00654 ad7[1] = 0.0;
00655 ad7[2] = 0.0;
00656 ad7[3] = 0.0;
00657 ad7[4] = ad10[1];
00658 ad7[5] = 0.0;
00659 ad7[6] = 0.0;
00660 ad7[7] = 0.0;
00661 ad7[8] = ad10[2];
00662
00663 print_mat(ad7);
00664 print_mat(ad5);
00665 print_mat(ad6);
00666
00667 mat_mul(ad5, ad7, ad8);
00668 mat_mul(ad8, ad6, ad8);
00669
00670 print_mat(ad8);
00671 xout = d12 * ad8[0] + d13 * ad8[1] + d14 * ad8[2];
00672 yout = d12 * ad8[3] + d13 * ad8[4] + d14 * ad8[5];
00673 zout = d12 * ad8[6] + d13 * ad8[7] + d14 * ad8[8];
00674 if (Math::abs(xin - xout) > EPS3 || Math::abs(xin - xout) > EPS3 || Math::abs(xin - xout) > EPS3) {
00675 std::cout << "\n ERROR in vec = " << xin << " " << yin << " " << zin << std::endl;
00676 std::cout << "\n ERROR result = " << xout << " " << yout << " " << zout << std::endl;
00677 }
00678 }
00679 } else {
00680 compute_qr(ad10, ad11, ad3, ad4, flag);
00681 }
00682 ad12[0] = ad10[0];
00683 ad12[1] = ad10[1];
00684 ad12[2] = ad10[2];
00685 if (almostEqual(Math::abs(ad12[0]), 1.0) && almostEqual(Math::abs(ad12[1]), 1.0) && almostEqual(Math::abs(ad12[2]), 1.0)) {
00686 for (int j = 0; j < 3; j++)
00687 if (ad12[j] < 0.0)
00688 l++;
00689
00690 if (l == 0 || l == 2) {
00691 scale[0] = scale[1] = scale[2] = 1.0;
00692 for (int k = 0; k < 9; k++)
00693 rot[k] = ad9[k];
00694
00695 return;
00696 }
00697 }
00698 transpose_mat(ad3, ad5);
00699 transpose_mat(ad4, ad6);
00700 svdReorder(ad, ad5, ad6, ad12, rot, scale, flag);
00701 }
00702
00703 double Matrix3SVD::d_sign(double d, double d1)
00704 {
00705 double d2 = d < 0.0 ? -d : d;
00706 return d1 < 0.0 ? -d2 : d2;
00707 }
00708
00709 void Matrix3SVD::mat_mul(double ad[], double ad1[], double ad2[])
00710 {
00711 double ad3[9];
00712 ad3[0] = ad[0] * ad1[0] + ad[1] * ad1[3] + ad[2] * ad1[6];
00713 ad3[1] = ad[0] * ad1[1] + ad[1] * ad1[4] + ad[2] * ad1[7];
00714 ad3[2] = ad[0] * ad1[2] + ad[1] * ad1[5] + ad[2] * ad1[8];
00715 ad3[3] = ad[3] * ad1[0] + ad[4] * ad1[3] + ad[5] * ad1[6];
00716 ad3[4] = ad[3] * ad1[1] + ad[4] * ad1[4] + ad[5] * ad1[7];
00717 ad3[5] = ad[3] * ad1[2] + ad[4] * ad1[5] + ad[5] * ad1[8];
00718 ad3[6] = ad[6] * ad1[0] + ad[7] * ad1[3] + ad[8] * ad1[6];
00719 ad3[7] = ad[6] * ad1[1] + ad[7] * ad1[4] + ad[8] * ad1[7];
00720 ad3[8] = ad[6] * ad1[2] + ad[7] * ad1[5] + ad[8] * ad1[8];
00721 for (int i = 0; i < 9; i++)
00722 ad2[i] = ad3[i];
00723
00724 }
00725
00726 double Matrix3SVD::max(double d, double d1)
00727 {
00728 if (d > d1)
00729 return d;
00730 else
00731 return d1;
00732 }
00733
00734 double Matrix3SVD::max3(double ad[])
00735 {
00736 if (ad[0] > ad[1])
00737 if (ad[0] > ad[2])
00738 return ad[0];
00739 else
00740 return ad[2];
00741 if (ad[1] > ad[2])
00742 return ad[1];
00743 else
00744 return ad[2];
00745 }
00746
00747 double Matrix3SVD::min(double d, double d1)
00748 {
00749 if (d < d1)
00750 return d;
00751 else
00752 return d1;
00753 }
00754
00755 void Matrix3SVD::print_det(double ad[])
00756 {
00757 double d = (ad[0] * ad[4] * ad[8] + ad[1] * ad[5] * ad[6] + ad[2] * ad[3] * ad[7]) - ad[2] * ad[4] * ad[6] - ad[0] * ad[5] * ad[7] - ad[1] * ad[3] * ad[8];
00758 std::cout << "det= " << d << std::endl;
00759 }
00760
00761 void Matrix3SVD::print_mat(double ad[])
00762 {
00763 for (int i = 0; i < 3; i++)
00764 std::cout << ad[i * 3] << " " << ad[i * 3 + 1] << " " << ad[i * 3 + 2] << "\n";
00765 std::cout << std::endl;
00766
00767 }
00768 void Matrix3SVD::svdReorder(double ad[], double ad1[], double ad2[], double ad3[], double ad4[], double scale[], bool flag)
00769 {
00770 int ai[3];
00771 double ad6[3];
00772 double ad7[9];
00773 if (ad3[0] < 0.0) {
00774 ad3[0] = -ad3[0];
00775 ad2[0] = -ad2[0];
00776 ad2[1] = -ad2[1];
00777 ad2[2] = -ad2[2];
00778 }
00779 if (ad3[1] < 0.0) {
00780 ad3[1] = -ad3[1];
00781 ad2[3] = -ad2[3];
00782 ad2[4] = -ad2[4];
00783 ad2[5] = -ad2[5];
00784 }
00785 if (ad3[2] < 0.0) {
00786 ad3[2] = -ad3[2];
00787 ad2[6] = -ad2[6];
00788 ad2[7] = -ad2[7];
00789 ad2[8] = -ad2[8];
00790 }
00791 mat_mul(ad1, ad2, ad7);
00792 if (almostEqual(Math::abs(ad3[0]), Math::abs(ad3[1])) && almostEqual(Math::abs(ad3[1]), Math::abs(ad3[2]))) {
00793 for (int j = 0; j < 9; j++)
00794 ad4[j] = ad7[j];
00795
00796 for (int k = 0; k < 3; k++)
00797 scale[k] = ad3[k];
00798
00799 } else {
00800 if (ad3[0] > ad3[1]) {
00801 if (ad3[0] > ad3[2]) {
00802 if (ad3[2] > ad3[1]) {
00803 ai[0] = 0;
00804 ai[1] = 2;
00805 ai[2] = 1;
00806 } else {
00807 ai[0] = 0;
00808 ai[1] = 1;
00809 ai[2] = 2;
00810 }
00811 } else {
00812 ai[0] = 2;
00813 ai[1] = 0;
00814 ai[2] = 1;
00815 }
00816 } else
00817 if (ad3[1] > ad3[2]) {
00818 if (ad3[2] > ad3[0]) {
00819 ai[0] = 1;
00820 ai[1] = 2;
00821 ai[2] = 0;
00822 } else {
00823 ai[0] = 1;
00824 ai[1] = 0;
00825 ai[2] = 2;
00826 }
00827 } else {
00828 ai[0] = 2;
00829 ai[1] = 1;
00830 ai[2] = 0;
00831 }
00832 ad6[0] = ad[0] * ad[0] + ad[1] * ad[1] + ad[2] * ad[2];
00833 ad6[1] = ad[3] * ad[3] + ad[4] * ad[4] + ad[5] * ad[5];
00834 ad6[2] = ad[6] * ad[6] + ad[7] * ad[7] + ad[8] * ad[8];
00835 unsigned char byte0;
00836 unsigned char byte1;
00837 unsigned char byte2;
00838 if (ad6[0] > ad6[1]) {
00839 if (ad6[0] > ad6[2]) {
00840 if (ad6[2] > ad6[1]) {
00841 byte0 = 0;
00842 byte2 = 1;
00843 byte1 = 2;
00844 } else {
00845 byte0 = 0;
00846 byte1 = 1;
00847 byte2 = 2;
00848 }
00849 } else {
00850 byte2 = 0;
00851 byte0 = 1;
00852 byte1 = 2;
00853 }
00854 } else
00855 if (ad6[1] > ad6[2]) {
00856 if (ad6[2] > ad6[0]) {
00857 byte1 = 0;
00858 byte2 = 1;
00859 byte0 = 2;
00860 } else {
00861 byte1 = 0;
00862 byte0 = 1;
00863 byte2 = 2;
00864 }
00865 } else {
00866 byte2 = 0;
00867 byte1 = 1;
00868 byte0 = 2;
00869 }
00870 int i = ai[byte0];
00871
00872 scale[0] = ad3[i];
00873 i = ai[byte1];
00874 scale[1] = ad3[i];
00875 i = ai[byte2];
00876 scale[2] = ad3[i];
00877
00878 i = ai[byte0];
00879 ad4[0] = ad7[i];
00880 i = ai[byte0] + 3;
00881 ad4[3] = ad7[i];
00882 i = ai[byte0] + 6;
00883 ad4[6] = ad7[i];
00884 i = ai[byte1];
00885 ad4[1] = ad7[i];
00886 i = ai[byte1] + 3;
00887 ad4[4] = ad7[i];
00888 i = ai[byte1] + 6;
00889 ad4[7] = ad7[i];
00890 i = ai[byte2];
00891 ad4[2] = ad7[i];
00892 i = ai[byte2] + 3;
00893 ad4[5] = ad7[i];
00894 i = ai[byte2] + 6;
00895 ad4[8] = ad7[i];
00896 }
00897 }
00898
00899 void Matrix3SVD::transpose_mat(double ad[], double ad1[])
00900 {
00901 ad1[0] = ad[0];
00902 ad1[1] = ad[3];
00903 ad1[2] = ad[6];
00904 ad1[3] = ad[1];
00905 ad1[4] = ad[4];
00906 ad1[5] = ad[7];
00907 ad1[6] = ad[2];
00908 ad1[7] = ad[5];
00909 ad1[8] = ad[8];
00910 }
00911
00912 void Matrix3SVD::copy(double ad[], double ad1[])
00913 {
00914 ad1[0] = ad[0];
00915 ad1[1] = ad[1];
00916 ad1[2] = ad[2];
00917 ad1[3] = ad[3];
00918 ad1[4] = ad[4];
00919 ad1[5] = ad[5];
00920 ad1[6] = ad[6];
00921 ad1[7] = ad[7];
00922 ad1[8] = ad[8];
00923 }
00924
00925 void Matrix3SVD::set(double c, double ad[])
00926 {
00927 ad[0] = c;
00928 ad[1] = 0.0;
00929 ad[2] = 0.0;
00930 ad[3] = 0.0;
00931 ad[4] = c;
00932 ad[5] = 0.0;
00933 ad[6] = 0.0;
00934 ad[7] = 0.0;
00935 ad[8] = c;
00936 }
00937
00938 void Matrix3SVD::test()
00939 {
00940 double ar[9];
00941 double cr[9];
00942 double rr[9];
00943 double sr[9];
00944
00945 for(int i = 0; i < 9; i++)
00946 ar[i] = cr[i] = rr[i] = sr[i] = 0.0;
00947
00948 ar[0] = 2 * 1.0;
00949 ar[1] = 0.0;
00950 ar[2] = 0.0;
00951 ar[3] = 0.0;
00952 ar[4] = 2 * cos(Math::toRadians(10));
00953 ar[5] = 2 * -sin(Math::toRadians(10));
00954 ar[6] = 0.0;
00955 ar[7] = 2 * sin(Math::toRadians(10));
00956 ar[8] = 2 * cos(Math::toRadians(10));
00957
00958 compute_svd(ar, sr, rr, true);
00959 print_mat(ar);
00960 print_mat(sr);
00961 print_mat(rr);
00962 }
00963
