00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041 #ifndef PQP_MATVEC_H
00042 #define PQP_MATVEC_H
00043
00044 #include <math.h>
00045 #include <stdio.h>
00046 #include "PQP_Compile.h"
00047
00048 #ifndef M_PI
00049 const PQP_REAL M_PI = (PQP_REAL)3.14159265359;
00050 #endif
00051
00052 #ifdef gnu
00053 #include "zzzz.h"
00054
00055 #ifdef hppa
00056 #define myfabs(x) \
00057 ({double __value, __arg = (x); \
00058 asm("fabs,dbl %1, %0": "=f" (__value): "f" (__arg)); \
00059 __value; \
00060 });
00061 #endif
00062
00063 #ifdef mips
00064 #define myfabs(x) \
00065 ({double __value, __arg = (x); \
00066 asm("abs.d %0, %1": "=f" (__value): "f" (__arg)); \
00067 __value; \
00068 });
00069 #endif
00070
00071 #else
00072
00073 #define myfabs(x) ((x < 0) ? -x : x)
00074
00075 #endif
00076
00077
00078 inline
00079 void
00080 Mprintg(const PQP_REAL M[3][3])
00081 {
00082 printf("%g %g %g\n%g %g %g\n%g %g %g\n",
00083 M[0][0], M[0][1], M[0][2],
00084 M[1][0], M[1][1], M[1][2],
00085 M[2][0], M[2][1], M[2][2]);
00086 }
00087
00088
00089 inline
00090 void
00091 Mfprint(FILE *f, const PQP_REAL M[3][3])
00092 {
00093 fprintf(f, "%g %g %g\n%g %g %g\n%g %g %g\n",
00094 M[0][0], M[0][1], M[0][2],
00095 M[1][0], M[1][1], M[1][2],
00096 M[2][0], M[2][1], M[2][2]);
00097 }
00098
00099 inline
00100 void
00101 Mprint(const PQP_REAL M[3][3])
00102 {
00103 printf("%g %g %g\n%g %g %g\n%g %g %g\n",
00104 M[0][0], M[0][1], M[0][2],
00105 M[1][0], M[1][1], M[1][2],
00106 M[2][0], M[2][1], M[2][2]);
00107 }
00108
00109 inline
00110 void
00111 Vprintg(const PQP_REAL V[3])
00112 {
00113 printf("%g %g %g\n", V[0], V[1], V[2]);
00114 }
00115
00116 inline
00117 void
00118 Vfprint(FILE *f, const PQP_REAL V[3])
00119 {
00120 fprintf(f, "%g %g %g\n", V[0], V[1], V[2]);
00121 }
00122
00123 inline
00124 void
00125 Vprint(const PQP_REAL V[3])
00126 {
00127 printf("%g %g %g\n", V[0], V[1], V[2]);
00128 }
00129
00130 inline
00131 void
00132 Midentity(PQP_REAL M[3][3])
00133 {
00134 M[0][0] = M[1][1] = M[2][2] = 1.0;
00135 M[0][1] = M[1][2] = M[2][0] = 0.0;
00136 M[0][2] = M[1][0] = M[2][1] = 0.0;
00137 }
00138
00139 inline
00140 void
00141 Videntity(PQP_REAL T[3])
00142 {
00143 T[0] = T[1] = T[2] = 0.0;
00144 }
00145
00146 inline
00147 void
00148 McM(PQP_REAL Mr[3][3], const PQP_REAL M[3][3])
00149 {
00150 Mr[0][0] = M[0][0]; Mr[0][1] = M[0][1]; Mr[0][2] = M[0][2];
00151 Mr[1][0] = M[1][0]; Mr[1][1] = M[1][1]; Mr[1][2] = M[1][2];
00152 Mr[2][0] = M[2][0]; Mr[2][1] = M[2][1]; Mr[2][2] = M[2][2];
00153 }
00154
00155 inline
00156 void
00157 MTcM(PQP_REAL Mr[3][3], const PQP_REAL M[3][3])
00158 {
00159 Mr[0][0] = M[0][0]; Mr[1][0] = M[0][1]; Mr[2][0] = M[0][2];
00160 Mr[0][1] = M[1][0]; Mr[1][1] = M[1][1]; Mr[2][1] = M[1][2];
00161 Mr[0][2] = M[2][0]; Mr[1][2] = M[2][1]; Mr[2][2] = M[2][2];
00162 }
00163
00164 inline
00165 void
00166 VcV(PQP_REAL Vr[3], const PQP_REAL V[3])
00167 {
00168 Vr[0] = V[0]; Vr[1] = V[1]; Vr[2] = V[2];
00169 }
00170
00171 inline
00172 void
00173 McolcV(PQP_REAL Vr[3], const PQP_REAL M[3][3], int c)
00174 {
00175 Vr[0] = M[0][c];
00176 Vr[1] = M[1][c];
00177 Vr[2] = M[2][c];
00178 }
00179
00180 inline
00181 void
00182 McolcMcol(PQP_REAL Mr[3][3], int cr, const PQP_REAL M[3][3], int c)
00183 {
00184 Mr[0][cr] = M[0][c];
00185 Mr[1][cr] = M[1][c];
00186 Mr[2][cr] = M[2][c];
00187 }
00188
00189 inline
00190 void
00191 MxMpV(PQP_REAL Mr[3][3], const PQP_REAL M1[3][3], const PQP_REAL M2[3][3], const PQP_REAL T[3])
00192 {
00193 Mr[0][0] = (M1[0][0] * M2[0][0] +
00194 M1[0][1] * M2[1][0] +
00195 M1[0][2] * M2[2][0] +
00196 T[0]);
00197 Mr[1][0] = (M1[1][0] * M2[0][0] +
00198 M1[1][1] * M2[1][0] +
00199 M1[1][2] * M2[2][0] +
00200 T[1]);
00201 Mr[2][0] = (M1[2][0] * M2[0][0] +
00202 M1[2][1] * M2[1][0] +
00203 M1[2][2] * M2[2][0] +
00204 T[2]);
00205 Mr[0][1] = (M1[0][0] * M2[0][1] +
00206 M1[0][1] * M2[1][1] +
00207 M1[0][2] * M2[2][1] +
00208 T[0]);
00209 Mr[1][1] = (M1[1][0] * M2[0][1] +
00210 M1[1][1] * M2[1][1] +
00211 M1[1][2] * M2[2][1] +
00212 T[1]);
00213 Mr[2][1] = (M1[2][0] * M2[0][1] +
00214 M1[2][1] * M2[1][1] +
00215 M1[2][2] * M2[2][1] +
00216 T[2]);
00217 Mr[0][2] = (M1[0][0] * M2[0][2] +
00218 M1[0][1] * M2[1][2] +
00219 M1[0][2] * M2[2][2] +
00220 T[0]);
00221 Mr[1][2] = (M1[1][0] * M2[0][2] +
00222 M1[1][1] * M2[1][2] +
00223 M1[1][2] * M2[2][2] +
00224 T[1]);
00225 Mr[2][2] = (M1[2][0] * M2[0][2] +
00226 M1[2][1] * M2[1][2] +
00227 M1[2][2] * M2[2][2] +
00228 T[2]);
00229 }
00230
00231 inline
00232 void
00233 MxM(PQP_REAL Mr[3][3], const PQP_REAL M1[3][3], const PQP_REAL M2[3][3])
00234 {
00235 Mr[0][0] = (M1[0][0] * M2[0][0] +
00236 M1[0][1] * M2[1][0] +
00237 M1[0][2] * M2[2][0]);
00238 Mr[1][0] = (M1[1][0] * M2[0][0] +
00239 M1[1][1] * M2[1][0] +
00240 M1[1][2] * M2[2][0]);
00241 Mr[2][0] = (M1[2][0] * M2[0][0] +
00242 M1[2][1] * M2[1][0] +
00243 M1[2][2] * M2[2][0]);
00244 Mr[0][1] = (M1[0][0] * M2[0][1] +
00245 M1[0][1] * M2[1][1] +
00246 M1[0][2] * M2[2][1]);
00247 Mr[1][1] = (M1[1][0] * M2[0][1] +
00248 M1[1][1] * M2[1][1] +
00249 M1[1][2] * M2[2][1]);
00250 Mr[2][1] = (M1[2][0] * M2[0][1] +
00251 M1[2][1] * M2[1][1] +
00252 M1[2][2] * M2[2][1]);
00253 Mr[0][2] = (M1[0][0] * M2[0][2] +
00254 M1[0][1] * M2[1][2] +
00255 M1[0][2] * M2[2][2]);
00256 Mr[1][2] = (M1[1][0] * M2[0][2] +
00257 M1[1][1] * M2[1][2] +
00258 M1[1][2] * M2[2][2]);
00259 Mr[2][2] = (M1[2][0] * M2[0][2] +
00260 M1[2][1] * M2[1][2] +
00261 M1[2][2] * M2[2][2]);
00262 }
00263
00264
00265 inline
00266 void
00267 MxMT(PQP_REAL Mr[3][3], const PQP_REAL M1[3][3], const PQP_REAL M2[3][3])
00268 {
00269 Mr[0][0] = (M1[0][0] * M2[0][0] +
00270 M1[0][1] * M2[0][1] +
00271 M1[0][2] * M2[0][2]);
00272 Mr[1][0] = (M1[1][0] * M2[0][0] +
00273 M1[1][1] * M2[0][1] +
00274 M1[1][2] * M2[0][2]);
00275 Mr[2][0] = (M1[2][0] * M2[0][0] +
00276 M1[2][1] * M2[0][1] +
00277 M1[2][2] * M2[0][2]);
00278 Mr[0][1] = (M1[0][0] * M2[1][0] +
00279 M1[0][1] * M2[1][1] +
00280 M1[0][2] * M2[1][2]);
00281 Mr[1][1] = (M1[1][0] * M2[1][0] +
00282 M1[1][1] * M2[1][1] +
00283 M1[1][2] * M2[1][2]);
00284 Mr[2][1] = (M1[2][0] * M2[1][0] +
00285 M1[2][1] * M2[1][1] +
00286 M1[2][2] * M2[1][2]);
00287 Mr[0][2] = (M1[0][0] * M2[2][0] +
00288 M1[0][1] * M2[2][1] +
00289 M1[0][2] * M2[2][2]);
00290 Mr[1][2] = (M1[1][0] * M2[2][0] +
00291 M1[1][1] * M2[2][1] +
00292 M1[1][2] * M2[2][2]);
00293 Mr[2][2] = (M1[2][0] * M2[2][0] +
00294 M1[2][1] * M2[2][1] +
00295 M1[2][2] * M2[2][2]);
00296 }
00297
00298 inline
00299 void
00300 MTxM(PQP_REAL Mr[3][3], const PQP_REAL M1[3][3], const PQP_REAL M2[3][3])
00301 {
00302 Mr[0][0] = (M1[0][0] * M2[0][0] +
00303 M1[1][0] * M2[1][0] +
00304 M1[2][0] * M2[2][0]);
00305 Mr[1][0] = (M1[0][1] * M2[0][0] +
00306 M1[1][1] * M2[1][0] +
00307 M1[2][1] * M2[2][0]);
00308 Mr[2][0] = (M1[0][2] * M2[0][0] +
00309 M1[1][2] * M2[1][0] +
00310 M1[2][2] * M2[2][0]);
00311 Mr[0][1] = (M1[0][0] * M2[0][1] +
00312 M1[1][0] * M2[1][1] +
00313 M1[2][0] * M2[2][1]);
00314 Mr[1][1] = (M1[0][1] * M2[0][1] +
00315 M1[1][1] * M2[1][1] +
00316 M1[2][1] * M2[2][1]);
00317 Mr[2][1] = (M1[0][2] * M2[0][1] +
00318 M1[1][2] * M2[1][1] +
00319 M1[2][2] * M2[2][1]);
00320 Mr[0][2] = (M1[0][0] * M2[0][2] +
00321 M1[1][0] * M2[1][2] +
00322 M1[2][0] * M2[2][2]);
00323 Mr[1][2] = (M1[0][1] * M2[0][2] +
00324 M1[1][1] * M2[1][2] +
00325 M1[2][1] * M2[2][2]);
00326 Mr[2][2] = (M1[0][2] * M2[0][2] +
00327 M1[1][2] * M2[1][2] +
00328 M1[2][2] * M2[2][2]);
00329 }
00330
00331 inline
00332 void
00333 MxV(PQP_REAL Vr[3], const PQP_REAL M1[3][3], const PQP_REAL V1[3])
00334 {
00335 Vr[0] = (M1[0][0] * V1[0] +
00336 M1[0][1] * V1[1] +
00337 M1[0][2] * V1[2]);
00338 Vr[1] = (M1[1][0] * V1[0] +
00339 M1[1][1] * V1[1] +
00340 M1[1][2] * V1[2]);
00341 Vr[2] = (M1[2][0] * V1[0] +
00342 M1[2][1] * V1[1] +
00343 M1[2][2] * V1[2]);
00344 }
00345
00346
00347 inline
00348 void
00349 MxVpV(PQP_REAL Vr[3], const PQP_REAL M1[3][3], const PQP_REAL V1[3], const PQP_REAL V2[3])
00350 {
00351 Vr[0] = (M1[0][0] * V1[0] +
00352 M1[0][1] * V1[1] +
00353 M1[0][2] * V1[2] +
00354 V2[0]);
00355 Vr[1] = (M1[1][0] * V1[0] +
00356 M1[1][1] * V1[1] +
00357 M1[1][2] * V1[2] +
00358 V2[1]);
00359 Vr[2] = (M1[2][0] * V1[0] +
00360 M1[2][1] * V1[1] +
00361 M1[2][2] * V1[2] +
00362 V2[2]);
00363 }
00364
00365
00366 inline
00367 void
00368 sMxVpV(PQP_REAL Vr[3], PQP_REAL s1, const PQP_REAL M1[3][3], const PQP_REAL V1[3], const PQP_REAL V2[3])
00369 {
00370 Vr[0] = s1 * (M1[0][0] * V1[0] +
00371 M1[0][1] * V1[1] +
00372 M1[0][2] * V1[2]) +
00373 V2[0];
00374 Vr[1] = s1 * (M1[1][0] * V1[0] +
00375 M1[1][1] * V1[1] +
00376 M1[1][2] * V1[2]) +
00377 V2[1];
00378 Vr[2] = s1 * (M1[2][0] * V1[0] +
00379 M1[2][1] * V1[1] +
00380 M1[2][2] * V1[2]) +
00381 V2[2];
00382 }
00383
00384 inline
00385 void
00386 MTxV(PQP_REAL Vr[3], const PQP_REAL M1[3][3], const PQP_REAL V1[3])
00387 {
00388 Vr[0] = (M1[0][0] * V1[0] +
00389 M1[1][0] * V1[1] +
00390 M1[2][0] * V1[2]);
00391 Vr[1] = (M1[0][1] * V1[0] +
00392 M1[1][1] * V1[1] +
00393 M1[2][1] * V1[2]);
00394 Vr[2] = (M1[0][2] * V1[0] +
00395 M1[1][2] * V1[1] +
00396 M1[2][2] * V1[2]);
00397 }
00398
00399 inline
00400 void
00401 sMTxV(PQP_REAL Vr[3], PQP_REAL s1, const PQP_REAL M1[3][3], const PQP_REAL V1[3])
00402 {
00403 Vr[0] = s1*(M1[0][0] * V1[0] +
00404 M1[1][0] * V1[1] +
00405 M1[2][0] * V1[2]);
00406 Vr[1] = s1*(M1[0][1] * V1[0] +
00407 M1[1][1] * V1[1] +
00408 M1[2][1] * V1[2]);
00409 Vr[2] = s1*(M1[0][2] * V1[0] +
00410 M1[1][2] * V1[1] +
00411 M1[2][2] * V1[2]);
00412 }
00413
00414 inline
00415 void
00416 sMxV(PQP_REAL Vr[3], PQP_REAL s1, const PQP_REAL M1[3][3], const PQP_REAL V1[3])
00417 {
00418 Vr[0] = s1*(M1[0][0] * V1[0] +
00419 M1[0][1] * V1[1] +
00420 M1[0][2] * V1[2]);
00421 Vr[1] = s1*(M1[1][0] * V1[0] +
00422 M1[1][1] * V1[1] +
00423 M1[1][2] * V1[2]);
00424 Vr[2] = s1*(M1[2][0] * V1[0] +
00425 M1[2][1] * V1[1] +
00426 M1[2][2] * V1[2]);
00427 }
00428
00429
00430 inline
00431 void
00432 VmV(PQP_REAL Vr[3], const PQP_REAL V1[3], const PQP_REAL V2[3])
00433 {
00434 Vr[0] = V1[0] - V2[0];
00435 Vr[1] = V1[1] - V2[1];
00436 Vr[2] = V1[2] - V2[2];
00437 }
00438
00439 inline
00440 void
00441 VpV(PQP_REAL Vr[3], const PQP_REAL V1[3], const PQP_REAL V2[3])
00442 {
00443 Vr[0] = V1[0] + V2[0];
00444 Vr[1] = V1[1] + V2[1];
00445 Vr[2] = V1[2] + V2[2];
00446 }
00447
00448 inline
00449 void
00450 VpVxS(PQP_REAL Vr[3], const PQP_REAL V1[3], const PQP_REAL V2[3], PQP_REAL s)
00451 {
00452 Vr[0] = V1[0] + V2[0] * s;
00453 Vr[1] = V1[1] + V2[1] * s;
00454 Vr[2] = V1[2] + V2[2] * s;
00455 }
00456
00457 inline
00458 void
00459 MskewV(PQP_REAL M[3][3], const PQP_REAL v[3])
00460 {
00461 M[0][0] = M[1][1] = M[2][2] = 0.0;
00462 M[1][0] = v[2];
00463 M[0][1] = -v[2];
00464 M[0][2] = v[1];
00465 M[2][0] = -v[1];
00466 M[1][2] = -v[0];
00467 M[2][1] = v[0];
00468 }
00469
00470
00471 inline
00472 void
00473 VcrossV(PQP_REAL Vr[3], const PQP_REAL V1[3], const PQP_REAL V2[3])
00474 {
00475 Vr[0] = V1[1]*V2[2] - V1[2]*V2[1];
00476 Vr[1] = V1[2]*V2[0] - V1[0]*V2[2];
00477 Vr[2] = V1[0]*V2[1] - V1[1]*V2[0];
00478 }
00479
00480 inline
00481 PQP_REAL
00482 Vlength(PQP_REAL V[3])
00483 {
00484 return sqrt(V[0]*V[0] + V[1]*V[1] + V[2]*V[2]);
00485 }
00486
00487 inline
00488 void
00489 Vnormalize(PQP_REAL V[3])
00490 {
00491 PQP_REAL d = (PQP_REAL)1.0 / sqrt(V[0]*V[0] + V[1]*V[1] + V[2]*V[2]);
00492 V[0] *= d;
00493 V[1] *= d;
00494 V[2] *= d;
00495 }
00496
00497 inline
00498 PQP_REAL
00499 VdotV(const PQP_REAL V1[3], const PQP_REAL V2[3])
00500 {
00501 return (V1[0]*V2[0] + V1[1]*V2[1] + V1[2]*V2[2]);
00502 }
00503
00504 inline
00505 PQP_REAL
00506 VdistV2(const PQP_REAL V1[3], const PQP_REAL V2[3])
00507 {
00508 return ( (V1[0]-V2[0]) * (V1[0]-V2[0]) +
00509 (V1[1]-V2[1]) * (V1[1]-V2[1]) +
00510 (V1[2]-V2[2]) * (V1[2]-V2[2]));
00511 }
00512
00513 inline
00514 void
00515 VxS(PQP_REAL Vr[3], const PQP_REAL V[3], PQP_REAL s)
00516 {
00517 Vr[0] = V[0] * s;
00518 Vr[1] = V[1] * s;
00519 Vr[2] = V[2] * s;
00520 }
00521
00522 inline
00523 void
00524 MRotZ(PQP_REAL Mr[3][3], PQP_REAL t)
00525 {
00526 Mr[0][0] = cos(t);
00527 Mr[1][0] = sin(t);
00528 Mr[0][1] = -Mr[1][0];
00529 Mr[1][1] = Mr[0][0];
00530 Mr[2][0] = Mr[2][1] = 0.0;
00531 Mr[0][2] = Mr[1][2] = 0.0;
00532 Mr[2][2] = 1.0;
00533 }
00534
00535 inline
00536 void
00537 MRotX(PQP_REAL Mr[3][3], PQP_REAL t)
00538 {
00539 Mr[1][1] = cos(t);
00540 Mr[2][1] = sin(t);
00541 Mr[1][2] = -Mr[2][1];
00542 Mr[2][2] = Mr[1][1];
00543 Mr[0][1] = Mr[0][2] = 0.0;
00544 Mr[1][0] = Mr[2][0] = 0.0;
00545 Mr[0][0] = 1.0;
00546 }
00547
00548 inline
00549 void
00550 MRotY(PQP_REAL Mr[3][3], PQP_REAL t)
00551 {
00552 Mr[2][2] = cos(t);
00553 Mr[0][2] = sin(t);
00554 Mr[2][0] = -Mr[0][2];
00555 Mr[0][0] = Mr[2][2];
00556 Mr[1][2] = Mr[1][0] = 0.0;
00557 Mr[2][1] = Mr[0][1] = 0.0;
00558 Mr[1][1] = 1.0;
00559 }
00560
00561 inline
00562 void
00563 MVtoOGL(double oglm[16], const PQP_REAL R[3][3], const PQP_REAL T[3])
00564 {
00565 oglm[0] = (double)R[0][0];
00566 oglm[1] = (double)R[1][0];
00567 oglm[2] = (double)R[2][0];
00568 oglm[3] = 0.0;
00569 oglm[4] = (double)R[0][1];
00570 oglm[5] = (double)R[1][1];
00571 oglm[6] = (double)R[2][1];
00572 oglm[7] = 0.0;
00573 oglm[8] = (double)R[0][2];
00574 oglm[9] = (double)R[1][2];
00575 oglm[10] = (double)R[2][2];
00576 oglm[11] = 0.0;
00577 oglm[12] = (double)T[0];
00578 oglm[13] = (double)T[1];
00579 oglm[14] = (double)T[2];
00580 oglm[15] = 1.0;
00581 }
00582
00583 inline
00584 void
00585 OGLtoMV(PQP_REAL R[3][3], PQP_REAL T[3], const double oglm[16])
00586 {
00587 R[0][0] = (PQP_REAL)oglm[0];
00588 R[1][0] = (PQP_REAL)oglm[1];
00589 R[2][0] = (PQP_REAL)oglm[2];
00590
00591 R[0][1] = (PQP_REAL)oglm[4];
00592 R[1][1] = (PQP_REAL)oglm[5];
00593 R[2][1] = (PQP_REAL)oglm[6];
00594
00595 R[0][2] = (PQP_REAL)oglm[8];
00596 R[1][2] = (PQP_REAL)oglm[9];
00597 R[2][2] = (PQP_REAL)oglm[10];
00598
00599 T[0] = (PQP_REAL)oglm[12];
00600 T[1] = (PQP_REAL)oglm[13];
00601 T[2] = (PQP_REAL)oglm[14];
00602 }
00603
00604
00605 const int QX = 0;
00606 const int QY = 1;
00607 const int QZ = 2;
00608 const int QW = 3;
00609
00610 inline
00611 void
00612 MRotQ(PQP_REAL destMatrix[3][3], PQP_REAL srcQuat[4])
00613 {
00614 PQP_REAL s;
00615 PQP_REAL xs, ys, zs,
00616 wx, wy, wz,
00617 xx, xy, xz,
00618 yy, yz, zz;
00619
00620
00621
00622
00623
00624
00625 s = (PQP_REAL)2.0 / (srcQuat[QX]*srcQuat[QX] + srcQuat[QY]*srcQuat[QY] +
00626 srcQuat[QZ]*srcQuat[QZ] + srcQuat[QW]*srcQuat[QW]);
00627
00628 xs = srcQuat[QX] * s; ys = srcQuat[QY] * s; zs = srcQuat[QZ] * s;
00629 wx = srcQuat[QW] * xs; wy = srcQuat[QW] * ys; wz = srcQuat[QW] * zs;
00630 xx = srcQuat[QX] * xs; xy = srcQuat[QX] * ys; xz = srcQuat[QX] * zs;
00631 yy = srcQuat[QY] * ys; yz = srcQuat[QY] * zs; zz = srcQuat[QZ] * zs;
00632
00633 destMatrix[QX][QX] = (PQP_REAL)1.0 - (yy + zz);
00634 destMatrix[QX][QY] = xy + wz;
00635 destMatrix[QX][QZ] = xz - wy;
00636
00637 destMatrix[QY][QX] = xy - wz;
00638 destMatrix[QY][QY] = (PQP_REAL)1.0 - (xx + zz);
00639 destMatrix[QY][QZ] = yz + wx;
00640
00641 destMatrix[QZ][QX] = xz + wy;
00642 destMatrix[QZ][QY] = yz - wx;
00643 destMatrix[QZ][QZ] = (PQP_REAL)1.0 - (xx + yy);
00644 }
00645
00646 inline
00647 void
00648 Mqinverse(PQP_REAL Mr[3][3], PQP_REAL m[3][3])
00649 {
00650 int i,j;
00651
00652 for(i=0; i<3; i++)
00653 for(j=0; j<3; j++)
00654 {
00655 int i1 = (i+1)%3;
00656 int i2 = (i+2)%3;
00657 int j1 = (j+1)%3;
00658 int j2 = (j+2)%3;
00659 Mr[i][j] = (m[j1][i1]*m[j2][i2] - m[j1][i2]*m[j2][i1]);
00660 }
00661 }
00662
00663
00664
00665 #if 0
00666
00667 #define rfabs(x) ((x < 0) ? -x : x)
00668
00669 #define ROT(a,i,j,k,l) g=a[i][j]; h=a[k][l]; a[i][j]=g-s*(h+g*tau); a[k][l]=h+s*(g-h*tau);
00670
00671 int
00672 inline
00673 Meigen(PQP_REAL vout[3][3], PQP_REAL dout[3], PQP_REAL a[3][3])
00674 {
00675 int i;
00676 PQP_REAL tresh,theta,tau,t,sm,s,h,g,c;
00677 int nrot;
00678 PQP_REAL b[3];
00679 PQP_REAL z[3];
00680 PQP_REAL v[3][3];
00681 PQP_REAL d[3];
00682
00683 v[0][0] = v[1][1] = v[2][2] = 1.0;
00684 v[0][1] = v[1][2] = v[2][0] = 0.0;
00685 v[0][2] = v[1][0] = v[2][1] = 0.0;
00686
00687 b[0] = a[0][0]; d[0] = a[0][0]; z[0] = 0.0;
00688 b[1] = a[1][1]; d[1] = a[1][1]; z[1] = 0.0;
00689 b[2] = a[2][2]; d[2] = a[2][2]; z[2] = 0.0;
00690
00691 nrot = 0;
00692
00693
00694 for(i=0; i<50; i++)
00695 {
00696
00697 printf("2\n");
00698
00699 sm=0.0; sm+=fabs(a[0][1]); sm+=fabs(a[0][2]); sm+=fabs(a[1][2]);
00700 if (sm == 0.0) { McM(vout,v); VcV(dout,d); return i; }
00701
00702 if (i < 3) tresh=0.2*sm/(3*3); else tresh=0.0;
00703
00704 {
00705 g = 100.0*rfabs(a[0][1]);
00706 if (i>3 && rfabs(d[0])+g==rfabs(d[0]) && rfabs(d[1])+g==rfabs(d[1]))
00707 a[0][1]=0.0;
00708 else if (rfabs(a[0][1])>tresh)
00709 {
00710 h = d[1]-d[0];
00711 if (rfabs(h)+g == rfabs(h)) t=(a[0][1])/h;
00712 else
00713 {
00714 theta=0.5*h/(a[0][1]);
00715 t=1.0/(rfabs(theta)+sqrt(1.0+theta*theta));
00716 if (theta < 0.0) t = -t;
00717 }
00718 c=1.0/sqrt(1+t*t); s=t*c; tau=s/(1.0+c); h=t*a[0][1];
00719 z[0] -= h; z[1] += h; d[0] -= h; d[1] += h;
00720 a[0][1]=0.0;
00721 ROT(a,0,2,1,2); ROT(v,0,0,0,1); ROT(v,1,0,1,1); ROT(v,2,0,2,1);
00722 nrot++;
00723 }
00724 }
00725
00726 {
00727 g = 100.0*rfabs(a[0][2]);
00728 if (i>3 && rfabs(d[0])+g==rfabs(d[0]) && rfabs(d[2])+g==rfabs(d[2]))
00729 a[0][2]=0.0;
00730 else if (rfabs(a[0][2])>tresh)
00731 {
00732 h = d[2]-d[0];
00733 if (rfabs(h)+g == rfabs(h)) t=(a[0][2])/h;
00734 else
00735 {
00736 theta=0.5*h/(a[0][2]);
00737 t=1.0/(rfabs(theta)+sqrt(1.0+theta*theta));
00738 if (theta < 0.0) t = -t;
00739 }
00740 c=1.0/sqrt(1+t*t); s=t*c; tau=s/(1.0+c); h=t*a[0][2];
00741 z[0] -= h; z[2] += h; d[0] -= h; d[2] += h;
00742 a[0][2]=0.0;
00743 ROT(a,0,1,1,2); ROT(v,0,0,0,2); ROT(v,1,0,1,2); ROT(v,2,0,2,2);
00744 nrot++;
00745 }
00746 }
00747
00748
00749 {
00750 g = 100.0*rfabs(a[1][2]);
00751 if (i>3 && rfabs(d[1])+g==rfabs(d[1]) && rfabs(d[2])+g==rfabs(d[2]))
00752 a[1][2]=0.0;
00753 else if (rfabs(a[1][2])>tresh)
00754 {
00755 h = d[2]-d[1];
00756 if (rfabs(h)+g == rfabs(h)) t=(a[1][2])/h;
00757 else
00758 {
00759 theta=0.5*h/(a[1][2]);
00760 t=1.0/(rfabs(theta)+sqrt(1.0+theta*theta));
00761 if (theta < 0.0) t = -t;
00762 }
00763 c=1.0/sqrt(1+t*t); s=t*c; tau=s/(1.0+c); h=t*a[1][2];
00764 z[1] -= h; z[2] += h; d[1] -= h; d[2] += h;
00765 a[1][2]=0.0;
00766 ROT(a,0,1,0,2); ROT(v,0,1,0,2); ROT(v,1,1,1,2); ROT(v,2,1,2,2);
00767 nrot++;
00768 }
00769 }
00770
00771 b[0] += z[0]; d[0] = b[0]; z[0] = 0.0;
00772 b[1] += z[1]; d[1] = b[1]; z[1] = 0.0;
00773 b[2] += z[2]; d[2] = b[2]; z[2] = 0.0;
00774
00775 }
00776
00777 fprintf(stderr, "eigen: too many iterations in Jacobi transform (%d).\n", i);
00778
00779 return i;
00780 }
00781
00782 #else
00783
00784
00785
00786 #define ROTATE(a,i,j,k,l) g=a[i][j]; h=a[k][l]; a[i][j]=g-s*(h+g*tau); a[k][l]=h+s*(g-h*tau);
00787
00788 void
00789 inline
00790 Meigen(PQP_REAL vout[3][3], PQP_REAL dout[3], PQP_REAL a[3][3])
00791 {
00792 int n = 3;
00793 int j,iq,ip,i;
00794 PQP_REAL tresh,theta,tau,t,sm,s,h,g,c;
00795 int nrot;
00796 PQP_REAL b[3];
00797 PQP_REAL z[3];
00798 PQP_REAL v[3][3];
00799 PQP_REAL d[3];
00800
00801 Midentity(v);
00802 for(ip=0; ip<n; ip++)
00803 {
00804 b[ip] = a[ip][ip];
00805 d[ip] = a[ip][ip];
00806 z[ip] = 0.0;
00807 }
00808
00809 nrot = 0;
00810
00811 for(i=0; i<50; i++)
00812 {
00813
00814 sm=0.0;
00815 for(ip=0;ip<n;ip++) for(iq=ip+1;iq<n;iq++) sm+=fabs(a[ip][iq]);
00816 if (sm == 0.0)
00817 {
00818 McM(vout, v);
00819 VcV(dout, d);
00820 return;
00821 }
00822
00823
00824 if (i < 3) tresh=(PQP_REAL)0.2*sm/(n*n);
00825 else tresh=0.0;
00826
00827 for(ip=0; ip<n; ip++) for(iq=ip+1; iq<n; iq++)
00828 {
00829 g = (PQP_REAL)100.0*fabs(a[ip][iq]);
00830 if (i>3 &&
00831 fabs(d[ip])+g==fabs(d[ip]) &&
00832 fabs(d[iq])+g==fabs(d[iq]))
00833 a[ip][iq]=0.0;
00834 else if (fabs(a[ip][iq])>tresh)
00835 {
00836 h = d[iq]-d[ip];
00837 if (fabs(h)+g == fabs(h)) t=(a[ip][iq])/h;
00838 else
00839 {
00840 theta=(PQP_REAL)0.5*h/(a[ip][iq]);
00841 t=(PQP_REAL)(1.0/(fabs(theta)+sqrt(1.0+theta*theta)));
00842 if (theta < 0.0) t = -t;
00843 }
00844 c=(PQP_REAL)1.0/sqrt(1+t*t);
00845 s=t*c;
00846 tau=s/((PQP_REAL)1.0+c);
00847 h=t*a[ip][iq];
00848 z[ip] -= h;
00849 z[iq] += h;
00850 d[ip] -= h;
00851 d[iq] += h;
00852 a[ip][iq]=0.0;
00853 for(j=0;j<ip;j++) { ROTATE(a,j,ip,j,iq); }
00854 for(j=ip+1;j<iq;j++) { ROTATE(a,ip,j,j,iq); }
00855 for(j=iq+1;j<n;j++) { ROTATE(a,ip,j,iq,j); }
00856 for(j=0;j<n;j++) { ROTATE(v,j,ip,j,iq); }
00857 nrot++;
00858 }
00859 }
00860 for(ip=0;ip<n;ip++)
00861 {
00862 b[ip] += z[ip];
00863 d[ip] = b[ip];
00864 z[ip] = 0.0;
00865 }
00866 }
00867
00868 fprintf(stderr, "eigen: too many iterations in Jacobi transform.\n");
00869
00870 return;
00871 }
00872
00873
00874 #endif
00875
00876 #endif
00877