libmove3d
3.13.0
|
00001 /*************************************************************************\ 00002 00003 Copyright 1999 The University of North Carolina at Chapel Hill. 00004 All Rights Reserved. 00005 00006 Permission to use, copy, modify and distribute this software and its 00007 documentation for educational, research and non-profit purposes, without 00008 fee, and without a written agreement is hereby granted, provided that the 00009 above copyright notice and the following three paragraphs appear in all 00010 copies. 00011 00012 IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL BE 00013 LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR 00014 CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE 00015 USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY 00016 OF NORTH CAROLINA HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH 00017 DAMAGES. 00018 00019 THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY 00020 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 00021 MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE 00022 PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF 00023 NORTH CAROLINA HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, 00024 UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 00025 00026 The authors may be contacted via: 00027 00028 US Mail: S. Gottschalk 00029 Department of Computer Science 00030 Sitterson Hall, CB #3175 00031 University of N. Carolina 00032 Chapel Hill, NC 27599-3175 00033 00034 Phone: (919)962-1749 00035 00036 EMail: geom@cs.unc.edu 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 // taken from quatlib, written by Richard Holloway 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 * For unit srcQuat, just set s = 2.0; or set xs = srcQuat[QX] + 00622 * srcQuat[QX], etc. 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 // Meigen from Numerical Recipes in C 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 // MATVEC_H