MatVec.h
Go to the documentation of this file.
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


jskeus
Author(s): JSK Alumnis
autogenerated on Tue Mar 7 2017 04:04:34