gim_linear_math.h
Go to the documentation of this file.
00001 #ifndef GIM_LINEAR_H_INCLUDED
00002 #define GIM_LINEAR_H_INCLUDED
00003 
00008 /*
00009 -----------------------------------------------------------------------------
00010 This source file is part of GIMPACT Library.
00011 
00012 For the latest info, see http://gimpact.sourceforge.net/
00013 
00014 Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
00015 email: projectileman@yahoo.com
00016 
00017  This library is free software; you can redistribute it and/or
00018  modify it under the terms of EITHER:
00019    (1) The GNU Lesser General Public License as published by the Free
00020        Software Foundation; either version 2.1 of the License, or (at
00021        your option) any later version. The text of the GNU Lesser
00022        General Public License is included with this library in the
00023        file GIMPACT-LICENSE-LGPL.TXT.
00024    (2) The BSD-style license that is included with this library in
00025        the file GIMPACT-LICENSE-BSD.TXT.
00026    (3) The zlib/libpng license that is included with this library in
00027        the file GIMPACT-LICENSE-ZLIB.TXT.
00028 
00029  This library is distributed in the hope that it will be useful,
00030  but WITHOUT ANY WARRANTY; without even the implied warranty of
00031  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
00032  GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
00033 
00034 -----------------------------------------------------------------------------
00035 */
00036 
00037 
00038 #include "gim_math.h"
00039 #include "gim_geom_types.h"
00040 
00041 
00042 
00043 
00045 #define VEC_ZERO_2(a)                           \
00046 {                                               \
00047    (a)[0] = (a)[1] = 0.0f;                      \
00048 }\
00049 
00050 
00052 #define VEC_ZERO(a)                             \
00053 {                                               \
00054    (a)[0] = (a)[1] = (a)[2] = 0.0f;             \
00055 }\
00056 
00057 
00059 #define VEC_ZERO_4(a)                           \
00060 {                                               \
00061    (a)[0] = (a)[1] = (a)[2] = (a)[3] = 0.0f;    \
00062 }\
00063 
00064 
00066 #define VEC_COPY_2(b,a)                         \
00067 {                                               \
00068    (b)[0] = (a)[0];                             \
00069    (b)[1] = (a)[1];                             \
00070 }\
00071 
00072 
00074 #define VEC_COPY(b,a)                           \
00075 {                                               \
00076    (b)[0] = (a)[0];                             \
00077    (b)[1] = (a)[1];                             \
00078    (b)[2] = (a)[2];                             \
00079 }\
00080 
00081 
00083 #define VEC_COPY_4(b,a)                         \
00084 {                                               \
00085    (b)[0] = (a)[0];                             \
00086    (b)[1] = (a)[1];                             \
00087    (b)[2] = (a)[2];                             \
00088    (b)[3] = (a)[3];                             \
00089 }\
00090 
00091 
00092 #define VEC_SWAP(b,a)                           \
00093 {  \
00094     GIM_SWAP_NUMBERS((b)[0],(a)[0]);\
00095     GIM_SWAP_NUMBERS((b)[1],(a)[1]);\
00096     GIM_SWAP_NUMBERS((b)[2],(a)[2]);\
00097 }\
00098 
00099 
00100 #define VEC_DIFF_2(v21,v2,v1)                   \
00101 {                                               \
00102    (v21)[0] = (v2)[0] - (v1)[0];                \
00103    (v21)[1] = (v2)[1] - (v1)[1];                \
00104 }\
00105 
00106 
00108 #define VEC_DIFF(v21,v2,v1)                     \
00109 {                                               \
00110    (v21)[0] = (v2)[0] - (v1)[0];                \
00111    (v21)[1] = (v2)[1] - (v1)[1];                \
00112    (v21)[2] = (v2)[2] - (v1)[2];                \
00113 }\
00114 
00115 
00117 #define VEC_DIFF_4(v21,v2,v1)                   \
00118 {                                               \
00119    (v21)[0] = (v2)[0] - (v1)[0];                \
00120    (v21)[1] = (v2)[1] - (v1)[1];                \
00121    (v21)[2] = (v2)[2] - (v1)[2];                \
00122    (v21)[3] = (v2)[3] - (v1)[3];                \
00123 }\
00124 
00125 
00127 #define VEC_SUM_2(v21,v2,v1)                    \
00128 {                                               \
00129    (v21)[0] = (v2)[0] + (v1)[0];                \
00130    (v21)[1] = (v2)[1] + (v1)[1];                \
00131 }\
00132 
00133 
00135 #define VEC_SUM(v21,v2,v1)                      \
00136 {                                               \
00137    (v21)[0] = (v2)[0] + (v1)[0];                \
00138    (v21)[1] = (v2)[1] + (v1)[1];                \
00139    (v21)[2] = (v2)[2] + (v1)[2];                \
00140 }\
00141 
00142 
00144 #define VEC_SUM_4(v21,v2,v1)                    \
00145 {                                               \
00146    (v21)[0] = (v2)[0] + (v1)[0];                \
00147    (v21)[1] = (v2)[1] + (v1)[1];                \
00148    (v21)[2] = (v2)[2] + (v1)[2];                \
00149    (v21)[3] = (v2)[3] + (v1)[3];                \
00150 }\
00151 
00152 
00154 #define VEC_SCALE_2(c,a,b)                      \
00155 {                                               \
00156    (c)[0] = (a)*(b)[0];                         \
00157    (c)[1] = (a)*(b)[1];                         \
00158 }\
00159 
00160 
00162 #define VEC_SCALE(c,a,b)                        \
00163 {                                               \
00164    (c)[0] = (a)*(b)[0];                         \
00165    (c)[1] = (a)*(b)[1];                         \
00166    (c)[2] = (a)*(b)[2];                         \
00167 }\
00168 
00169 
00171 #define VEC_SCALE_4(c,a,b)                      \
00172 {                                               \
00173    (c)[0] = (a)*(b)[0];                         \
00174    (c)[1] = (a)*(b)[1];                         \
00175    (c)[2] = (a)*(b)[2];                         \
00176    (c)[3] = (a)*(b)[3];                         \
00177 }\
00178 
00179 
00181 #define VEC_ACCUM_2(c,a,b)                      \
00182 {                                               \
00183    (c)[0] += (a)*(b)[0];                        \
00184    (c)[1] += (a)*(b)[1];                        \
00185 }\
00186 
00187 
00189 #define VEC_ACCUM(c,a,b)                        \
00190 {                                               \
00191    (c)[0] += (a)*(b)[0];                        \
00192    (c)[1] += (a)*(b)[1];                        \
00193    (c)[2] += (a)*(b)[2];                        \
00194 }\
00195 
00196 
00198 #define VEC_ACCUM_4(c,a,b)                      \
00199 {                                               \
00200    (c)[0] += (a)*(b)[0];                        \
00201    (c)[1] += (a)*(b)[1];                        \
00202    (c)[2] += (a)*(b)[2];                        \
00203    (c)[3] += (a)*(b)[3];                        \
00204 }\
00205 
00206 
00208 #define VEC_DOT_2(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1])
00209 
00210 
00212 #define VEC_DOT(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2])
00213 
00215 #define VEC_DOT_4(a,b)  ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2] + (a)[3]*(b)[3])
00216 
00218 #define VEC_IMPACT_SQ(bsq,direction,position) {\
00219    GREAL _llel_ = VEC_DOT(direction, position);\
00220    bsq = VEC_DOT(position, position) - _llel_*_llel_;\
00221 }\
00222 
00223 
00225 #define VEC_IMPACT(bsq,direction,position)      {\
00226    VEC_IMPACT_SQ(bsq,direction,position);               \
00227    GIM_SQRT(bsq,bsq);                                   \
00228 }\
00229 
00230 
00231 #define VEC_LENGTH_2(a,l)\
00232 {\
00233     GREAL _pp = VEC_DOT_2(a,a);\
00234     GIM_SQRT(_pp,l);\
00235 }\
00236 
00237 
00239 #define VEC_LENGTH(a,l)\
00240 {\
00241     GREAL _pp = VEC_DOT(a,a);\
00242     GIM_SQRT(_pp,l);\
00243 }\
00244 
00245 
00247 #define VEC_LENGTH_4(a,l)\
00248 {\
00249     GREAL _pp = VEC_DOT_4(a,a);\
00250     GIM_SQRT(_pp,l);\
00251 }\
00252 
00253 
00254 #define VEC_INV_LENGTH_2(a,l)\
00255 {\
00256     GREAL _pp = VEC_DOT_2(a,a);\
00257     GIM_INV_SQRT(_pp,l);\
00258 }\
00259 
00260 
00262 #define VEC_INV_LENGTH(a,l)\
00263 {\
00264     GREAL _pp = VEC_DOT(a,a);\
00265     GIM_INV_SQRT(_pp,l);\
00266 }\
00267 
00268 
00270 #define VEC_INV_LENGTH_4(a,l)\
00271 {\
00272     GREAL _pp = VEC_DOT_4(a,a);\
00273     GIM_INV_SQRT(_pp,l);\
00274 }\
00275 
00276 
00277 
00279 #define VEC_DISTANCE(_len,_va,_vb) {\
00280     vec3f _tmp_;                                \
00281     VEC_DIFF(_tmp_, _vb, _va);                  \
00282     VEC_LENGTH(_tmp_,_len);                     \
00283 }\
00284 
00285 
00287 #define VEC_CONJUGATE_LENGTH(a,l)\
00288 {\
00289     GREAL _pp = 1.0 - a[0]*a[0] - a[1]*a[1] - a[2]*a[2];\
00290     GIM_SQRT(_pp,l);\
00291 }\
00292 
00293 
00295 #define VEC_NORMALIZE(a) {      \
00296     GREAL len;\
00297     VEC_INV_LENGTH(a,len); \
00298     if(len<G_REAL_INFINITY)\
00299     {\
00300         a[0] *= len;                            \
00301         a[1] *= len;                            \
00302         a[2] *= len;                            \
00303     }                                           \
00304 }\
00305 
00306 
00307 #define VEC_RENORMALIZE(a,newlen) {     \
00308     GREAL len;\
00309     VEC_INV_LENGTH(a,len); \
00310     if(len<G_REAL_INFINITY)\
00311     {\
00312         len *= newlen;\
00313         a[0] *= len;                            \
00314         a[1] *= len;                            \
00315         a[2] *= len;                            \
00316     }                                           \
00317 }\
00318 
00319 
00320 #define VEC_CROSS(c,a,b)                \
00321 {                                               \
00322    c[0] = (a)[1] * (b)[2] - (a)[2] * (b)[1];    \
00323    c[1] = (a)[2] * (b)[0] - (a)[0] * (b)[2];    \
00324    c[2] = (a)[0] * (b)[1] - (a)[1] * (b)[0];    \
00325 }\
00326 
00327 
00330 #define VEC_PERPENDICULAR(vp,v,n)                       \
00331 {                                               \
00332    GREAL dot = VEC_DOT(v, n);                   \
00333    vp[0] = (v)[0] - dot*(n)[0];         \
00334    vp[1] = (v)[1] - dot*(n)[1];         \
00335    vp[2] = (v)[2] - dot*(n)[2];         \
00336 }\
00337 
00338 
00340 #define VEC_PARALLEL(vp,v,n)                    \
00341 {                                               \
00342    GREAL dot = VEC_DOT(v, n);                   \
00343    vp[0] = (dot) * (n)[0];                      \
00344    vp[1] = (dot) * (n)[1];                      \
00345    vp[2] = (dot) * (n)[2];                      \
00346 }\
00347 
00348 
00350 #define VEC_PROJECT(vp,v,n)                     \
00351 { \
00352         GREAL scalar = VEC_DOT(v, n);                   \
00353         scalar/= VEC_DOT(n, n); \
00354         vp[0] = (scalar) * (n)[0];                      \
00355     vp[1] = (scalar) * (n)[1];                  \
00356     vp[2] = (scalar) * (n)[2];                  \
00357 }\
00358 
00359 
00361 #define VEC_UNPROJECT(vp,v,n)                   \
00362 { \
00363         GREAL scalar = VEC_DOT(v, n);                   \
00364         scalar = VEC_DOT(n, n)/scalar; \
00365         vp[0] = (scalar) * (n)[0];                      \
00366     vp[1] = (scalar) * (n)[1];                  \
00367     vp[2] = (scalar) * (n)[2];                  \
00368 }\
00369 
00370 
00373 #define VEC_REFLECT(vr,v,n)                     \
00374 {                                               \
00375    GREAL dot = VEC_DOT(v, n);                   \
00376    vr[0] = (v)[0] - 2.0 * (dot) * (n)[0];       \
00377    vr[1] = (v)[1] - 2.0 * (dot) * (n)[1];       \
00378    vr[2] = (v)[2] - 2.0 * (dot) * (n)[2];       \
00379 }\
00380 
00381 
00384 #define VEC_BLEND_AB(vr,sa,a,sb,b)                      \
00385 {                                               \
00386    vr[0] = (sa) * (a)[0] + (sb) * (b)[0];       \
00387    vr[1] = (sa) * (a)[1] + (sb) * (b)[1];       \
00388    vr[2] = (sa) * (a)[2] + (sb) * (b)[2];       \
00389 }\
00390 
00391 
00393 #define VEC_BLEND(vr,a,b,s) VEC_BLEND_AB(vr,(1-s),a,s,b)
00394 
00395 #define VEC_SET3(a,b,op,c) a[0]=b[0] op c[0]; a[1]=b[1] op c[1]; a[2]=b[2] op c[2];
00396 
00398 #define VEC_MAYOR_COORD(vec, maxc)\
00399 {\
00400         GREAL A[] = {fabs(vec[0]),fabs(vec[1]),fabs(vec[2])};\
00401     maxc =  A[0]>A[1]?(A[0]>A[2]?0:2):(A[1]>A[2]?1:2);\
00402 }\
00403 
00404 
00405 #define VEC_MINOR_AXES(vec, i0, i1)\
00406 {\
00407         VEC_MAYOR_COORD(vec,i0);\
00408         i0 = (i0+1)%3;\
00409         i1 = (i0+1)%3;\
00410 }\
00411 
00412 
00413 
00414 
00415 #define VEC_EQUAL(v1,v2) (v1[0]==v2[0]&&v1[1]==v2[1]&&v1[2]==v2[2])
00416 
00417 #define VEC_NEAR_EQUAL(v1,v2) (GIM_NEAR_EQUAL(v1[0],v2[0])&&GIM_NEAR_EQUAL(v1[1],v2[1])&&GIM_NEAR_EQUAL(v1[2],v2[2]))
00418 
00419 
00421 #define X_AXIS_CROSS_VEC(dst,src)\
00422 {                                          \
00423         dst[0] = 0.0f;     \
00424         dst[1] = -src[2];  \
00425         dst[2] = src[1];  \
00426 }\
00427 
00428 #define Y_AXIS_CROSS_VEC(dst,src)\
00429 {                                          \
00430         dst[0] = src[2];     \
00431         dst[1] = 0.0f;  \
00432         dst[2] = -src[0];  \
00433 }\
00434 
00435 #define Z_AXIS_CROSS_VEC(dst,src)\
00436 {                                          \
00437         dst[0] = -src[1];     \
00438         dst[1] = src[0];  \
00439         dst[2] = 0.0f;  \
00440 }\
00441 
00442 
00443 
00444 
00445 
00446 
00448 #define IDENTIFY_MATRIX_3X3(m)                  \
00449 {                                               \
00450    m[0][0] = 1.0;                               \
00451    m[0][1] = 0.0;                               \
00452    m[0][2] = 0.0;                               \
00453                                                 \
00454    m[1][0] = 0.0;                               \
00455    m[1][1] = 1.0;                               \
00456    m[1][2] = 0.0;                               \
00457                                                 \
00458    m[2][0] = 0.0;                               \
00459    m[2][1] = 0.0;                               \
00460    m[2][2] = 1.0;                               \
00461 }\
00462 
00463 
00464 #define IDENTIFY_MATRIX_4X4(m)                  \
00465 {                                               \
00466    m[0][0] = 1.0;                               \
00467    m[0][1] = 0.0;                               \
00468    m[0][2] = 0.0;                               \
00469    m[0][3] = 0.0;                               \
00470                                                 \
00471    m[1][0] = 0.0;                               \
00472    m[1][1] = 1.0;                               \
00473    m[1][2] = 0.0;                               \
00474    m[1][3] = 0.0;                               \
00475                                                 \
00476    m[2][0] = 0.0;                               \
00477    m[2][1] = 0.0;                               \
00478    m[2][2] = 1.0;                               \
00479    m[2][3] = 0.0;                               \
00480                                                 \
00481    m[3][0] = 0.0;                               \
00482    m[3][1] = 0.0;                               \
00483    m[3][2] = 0.0;                               \
00484    m[3][3] = 1.0;                               \
00485 }\
00486 
00487 
00488 #define ZERO_MATRIX_4X4(m)                      \
00489 {                                               \
00490    m[0][0] = 0.0;                               \
00491    m[0][1] = 0.0;                               \
00492    m[0][2] = 0.0;                               \
00493    m[0][3] = 0.0;                               \
00494                                                 \
00495    m[1][0] = 0.0;                               \
00496    m[1][1] = 0.0;                               \
00497    m[1][2] = 0.0;                               \
00498    m[1][3] = 0.0;                               \
00499                                                 \
00500    m[2][0] = 0.0;                               \
00501    m[2][1] = 0.0;                               \
00502    m[2][2] = 0.0;                               \
00503    m[2][3] = 0.0;                               \
00504                                                 \
00505    m[3][0] = 0.0;                               \
00506    m[3][1] = 0.0;                               \
00507    m[3][2] = 0.0;                               \
00508    m[3][3] = 0.0;                               \
00509 }\
00510 
00511 
00512 #define ROTX_CS(m,cosine,sine)          \
00513 {                                       \
00514    /* rotation about the x-axis */      \
00515                                         \
00516    m[0][0] = 1.0;                       \
00517    m[0][1] = 0.0;                       \
00518    m[0][2] = 0.0;                       \
00519    m[0][3] = 0.0;                       \
00520                                         \
00521    m[1][0] = 0.0;                       \
00522    m[1][1] = (cosine);                  \
00523    m[1][2] = (sine);                    \
00524    m[1][3] = 0.0;                       \
00525                                         \
00526    m[2][0] = 0.0;                       \
00527    m[2][1] = -(sine);                   \
00528    m[2][2] = (cosine);                  \
00529    m[2][3] = 0.0;                       \
00530                                         \
00531    m[3][0] = 0.0;                       \
00532    m[3][1] = 0.0;                       \
00533    m[3][2] = 0.0;                       \
00534    m[3][3] = 1.0;                       \
00535 }\
00536 
00537 
00538 #define ROTY_CS(m,cosine,sine)          \
00539 {                                       \
00540    /* rotation about the y-axis */      \
00541                                         \
00542    m[0][0] = (cosine);                  \
00543    m[0][1] = 0.0;                       \
00544    m[0][2] = -(sine);                   \
00545    m[0][3] = 0.0;                       \
00546                                         \
00547    m[1][0] = 0.0;                       \
00548    m[1][1] = 1.0;                       \
00549    m[1][2] = 0.0;                       \
00550    m[1][3] = 0.0;                       \
00551                                         \
00552    m[2][0] = (sine);                    \
00553    m[2][1] = 0.0;                       \
00554    m[2][2] = (cosine);                  \
00555    m[2][3] = 0.0;                       \
00556                                         \
00557    m[3][0] = 0.0;                       \
00558    m[3][1] = 0.0;                       \
00559    m[3][2] = 0.0;                       \
00560    m[3][3] = 1.0;                       \
00561 }\
00562 
00563 
00564 #define ROTZ_CS(m,cosine,sine)          \
00565 {                                       \
00566    /* rotation about the z-axis */      \
00567                                         \
00568    m[0][0] = (cosine);                  \
00569    m[0][1] = (sine);                    \
00570    m[0][2] = 0.0;                       \
00571    m[0][3] = 0.0;                       \
00572                                         \
00573    m[1][0] = -(sine);                   \
00574    m[1][1] = (cosine);                  \
00575    m[1][2] = 0.0;                       \
00576    m[1][3] = 0.0;                       \
00577                                         \
00578    m[2][0] = 0.0;                       \
00579    m[2][1] = 0.0;                       \
00580    m[2][2] = 1.0;                       \
00581    m[2][3] = 0.0;                       \
00582                                         \
00583    m[3][0] = 0.0;                       \
00584    m[3][1] = 0.0;                       \
00585    m[3][2] = 0.0;                       \
00586    m[3][3] = 1.0;                       \
00587 }\
00588 
00589 
00590 #define COPY_MATRIX_2X2(b,a)    \
00591 {                               \
00592    b[0][0] = a[0][0];           \
00593    b[0][1] = a[0][1];           \
00594                                 \
00595    b[1][0] = a[1][0];           \
00596    b[1][1] = a[1][1];           \
00597                                 \
00598 }\
00599 
00600 
00602 #define COPY_MATRIX_2X3(b,a)    \
00603 {                               \
00604    b[0][0] = a[0][0];           \
00605    b[0][1] = a[0][1];           \
00606    b[0][2] = a[0][2];           \
00607                                 \
00608    b[1][0] = a[1][0];           \
00609    b[1][1] = a[1][1];           \
00610    b[1][2] = a[1][2];           \
00611 }\
00612 
00613 
00615 #define COPY_MATRIX_3X3(b,a)    \
00616 {                               \
00617    b[0][0] = a[0][0];           \
00618    b[0][1] = a[0][1];           \
00619    b[0][2] = a[0][2];           \
00620                                 \
00621    b[1][0] = a[1][0];           \
00622    b[1][1] = a[1][1];           \
00623    b[1][2] = a[1][2];           \
00624                                 \
00625    b[2][0] = a[2][0];           \
00626    b[2][1] = a[2][1];           \
00627    b[2][2] = a[2][2];           \
00628 }\
00629 
00630 
00632 #define COPY_MATRIX_4X4(b,a)    \
00633 {                               \
00634    b[0][0] = a[0][0];           \
00635    b[0][1] = a[0][1];           \
00636    b[0][2] = a[0][2];           \
00637    b[0][3] = a[0][3];           \
00638                                 \
00639    b[1][0] = a[1][0];           \
00640    b[1][1] = a[1][1];           \
00641    b[1][2] = a[1][2];           \
00642    b[1][3] = a[1][3];           \
00643                                 \
00644    b[2][0] = a[2][0];           \
00645    b[2][1] = a[2][1];           \
00646    b[2][2] = a[2][2];           \
00647    b[2][3] = a[2][3];           \
00648                                 \
00649    b[3][0] = a[3][0];           \
00650    b[3][1] = a[3][1];           \
00651    b[3][2] = a[3][2];           \
00652    b[3][3] = a[3][3];           \
00653 }\
00654 
00655 
00657 #define TRANSPOSE_MATRIX_2X2(b,a)       \
00658 {                               \
00659    b[0][0] = a[0][0];           \
00660    b[0][1] = a[1][0];           \
00661                                 \
00662    b[1][0] = a[0][1];           \
00663    b[1][1] = a[1][1];           \
00664 }\
00665 
00666 
00668 #define TRANSPOSE_MATRIX_3X3(b,a)       \
00669 {                               \
00670    b[0][0] = a[0][0];           \
00671    b[0][1] = a[1][0];           \
00672    b[0][2] = a[2][0];           \
00673                                 \
00674    b[1][0] = a[0][1];           \
00675    b[1][1] = a[1][1];           \
00676    b[1][2] = a[2][1];           \
00677                                 \
00678    b[2][0] = a[0][2];           \
00679    b[2][1] = a[1][2];           \
00680    b[2][2] = a[2][2];           \
00681 }\
00682 
00683 
00685 #define TRANSPOSE_MATRIX_4X4(b,a)       \
00686 {                               \
00687    b[0][0] = a[0][0];           \
00688    b[0][1] = a[1][0];           \
00689    b[0][2] = a[2][0];           \
00690    b[0][3] = a[3][0];           \
00691                                 \
00692    b[1][0] = a[0][1];           \
00693    b[1][1] = a[1][1];           \
00694    b[1][2] = a[2][1];           \
00695    b[1][3] = a[3][1];           \
00696                                 \
00697    b[2][0] = a[0][2];           \
00698    b[2][1] = a[1][2];           \
00699    b[2][2] = a[2][2];           \
00700    b[2][3] = a[3][2];           \
00701                                 \
00702    b[3][0] = a[0][3];           \
00703    b[3][1] = a[1][3];           \
00704    b[3][2] = a[2][3];           \
00705    b[3][3] = a[3][3];           \
00706 }\
00707 
00708 
00710 #define SCALE_MATRIX_2X2(b,s,a)         \
00711 {                                       \
00712    b[0][0] = (s) * a[0][0];             \
00713    b[0][1] = (s) * a[0][1];             \
00714                                         \
00715    b[1][0] = (s) * a[1][0];             \
00716    b[1][1] = (s) * a[1][1];             \
00717 }\
00718 
00719 
00721 #define SCALE_MATRIX_3X3(b,s,a)         \
00722 {                                       \
00723    b[0][0] = (s) * a[0][0];             \
00724    b[0][1] = (s) * a[0][1];             \
00725    b[0][2] = (s) * a[0][2];             \
00726                                         \
00727    b[1][0] = (s) * a[1][0];             \
00728    b[1][1] = (s) * a[1][1];             \
00729    b[1][2] = (s) * a[1][2];             \
00730                                         \
00731    b[2][0] = (s) * a[2][0];             \
00732    b[2][1] = (s) * a[2][1];             \
00733    b[2][2] = (s) * a[2][2];             \
00734 }\
00735 
00736 
00738 #define SCALE_MATRIX_4X4(b,s,a)         \
00739 {                                       \
00740    b[0][0] = (s) * a[0][0];             \
00741    b[0][1] = (s) * a[0][1];             \
00742    b[0][2] = (s) * a[0][2];             \
00743    b[0][3] = (s) * a[0][3];             \
00744                                         \
00745    b[1][0] = (s) * a[1][0];             \
00746    b[1][1] = (s) * a[1][1];             \
00747    b[1][2] = (s) * a[1][2];             \
00748    b[1][3] = (s) * a[1][3];             \
00749                                         \
00750    b[2][0] = (s) * a[2][0];             \
00751    b[2][1] = (s) * a[2][1];             \
00752    b[2][2] = (s) * a[2][2];             \
00753    b[2][3] = (s) * a[2][3];             \
00754                                         \
00755    b[3][0] = s * a[3][0];               \
00756    b[3][1] = s * a[3][1];               \
00757    b[3][2] = s * a[3][2];               \
00758    b[3][3] = s * a[3][3];               \
00759 }\
00760 
00761 
00763 #define SCALE_VEC_MATRIX_2X2(b,svec,a)          \
00764 {                                       \
00765    b[0][0] = svec[0] * a[0][0];         \
00766    b[1][0] = svec[0] * a[1][0];         \
00767                                         \
00768    b[0][1] = svec[1] * a[0][1];         \
00769    b[1][1] = svec[1] * a[1][1];         \
00770 }\
00771 
00772 
00774 #define SCALE_VEC_MATRIX_3X3(b,svec,a)          \
00775 {                                       \
00776    b[0][0] = svec[0] * a[0][0];         \
00777    b[1][0] = svec[0] * a[1][0];         \
00778    b[2][0] = svec[0] * a[2][0];         \
00779                                         \
00780    b[0][1] = svec[1] * a[0][1];         \
00781    b[1][1] = svec[1] * a[1][1];         \
00782    b[2][1] = svec[1] * a[2][1];         \
00783                                         \
00784    b[0][2] = svec[2] * a[0][2];         \
00785    b[1][2] = svec[2] * a[1][2];         \
00786    b[2][2] = svec[2] * a[2][2];         \
00787 }\
00788 
00789 
00791 #define SCALE_VEC_MATRIX_4X4(b,svec,a)          \
00792 {                                       \
00793    b[0][0] = svec[0] * a[0][0];         \
00794    b[1][0] = svec[0] * a[1][0];         \
00795    b[2][0] = svec[0] * a[2][0];         \
00796    b[3][0] = svec[0] * a[3][0];         \
00797                                         \
00798    b[0][1] = svec[1] * a[0][1];         \
00799    b[1][1] = svec[1] * a[1][1];         \
00800    b[2][1] = svec[1] * a[2][1];         \
00801    b[3][1] = svec[1] * a[3][1];         \
00802                                         \
00803    b[0][2] = svec[2] * a[0][2];         \
00804    b[1][2] = svec[2] * a[1][2];         \
00805    b[2][2] = svec[2] * a[2][2];         \
00806    b[3][2] = svec[2] * a[3][2];         \
00807    \
00808    b[0][3] = svec[3] * a[0][3];         \
00809    b[1][3] = svec[3] * a[1][3];         \
00810    b[2][3] = svec[3] * a[2][3];         \
00811    b[3][3] = svec[3] * a[3][3];         \
00812 }\
00813 
00814 
00816 #define ACCUM_SCALE_MATRIX_2X2(b,s,a)           \
00817 {                                       \
00818    b[0][0] += (s) * a[0][0];            \
00819    b[0][1] += (s) * a[0][1];            \
00820                                         \
00821    b[1][0] += (s) * a[1][0];            \
00822    b[1][1] += (s) * a[1][1];            \
00823 }\
00824 
00825 
00827 #define ACCUM_SCALE_MATRIX_3X3(b,s,a)           \
00828 {                                       \
00829    b[0][0] += (s) * a[0][0];            \
00830    b[0][1] += (s) * a[0][1];            \
00831    b[0][2] += (s) * a[0][2];            \
00832                                         \
00833    b[1][0] += (s) * a[1][0];            \
00834    b[1][1] += (s) * a[1][1];            \
00835    b[1][2] += (s) * a[1][2];            \
00836                                         \
00837    b[2][0] += (s) * a[2][0];            \
00838    b[2][1] += (s) * a[2][1];            \
00839    b[2][2] += (s) * a[2][2];            \
00840 }\
00841 
00842 
00844 #define ACCUM_SCALE_MATRIX_4X4(b,s,a)           \
00845 {                                       \
00846    b[0][0] += (s) * a[0][0];            \
00847    b[0][1] += (s) * a[0][1];            \
00848    b[0][2] += (s) * a[0][2];            \
00849    b[0][3] += (s) * a[0][3];            \
00850                                         \
00851    b[1][0] += (s) * a[1][0];            \
00852    b[1][1] += (s) * a[1][1];            \
00853    b[1][2] += (s) * a[1][2];            \
00854    b[1][3] += (s) * a[1][3];            \
00855                                         \
00856    b[2][0] += (s) * a[2][0];            \
00857    b[2][1] += (s) * a[2][1];            \
00858    b[2][2] += (s) * a[2][2];            \
00859    b[2][3] += (s) * a[2][3];            \
00860                                         \
00861    b[3][0] += (s) * a[3][0];            \
00862    b[3][1] += (s) * a[3][1];            \
00863    b[3][2] += (s) * a[3][2];            \
00864    b[3][3] += (s) * a[3][3];            \
00865 }\
00866 
00867 
00869 #define MATRIX_PRODUCT_2X2(c,a,b)               \
00870 {                                               \
00871    c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0];   \
00872    c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1];   \
00873                                                 \
00874    c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0];   \
00875    c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1];   \
00876                                                 \
00877 }\
00878 
00879 
00881 #define MATRIX_PRODUCT_3X3(c,a,b)                               \
00882 {                                                               \
00883    c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0];   \
00884    c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1];   \
00885    c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2];   \
00886                                                                 \
00887    c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0];   \
00888    c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1];   \
00889    c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2];   \
00890                                                                 \
00891    c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0];   \
00892    c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1];   \
00893    c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2];   \
00894 }\
00895 
00896 
00899 #define MATRIX_PRODUCT_4X4(c,a,b)               \
00900 {                                               \
00901    c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0]+a[0][3]*b[3][0];\
00902    c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1]+a[0][3]*b[3][1];\
00903    c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2]+a[0][3]*b[3][2];\
00904    c[0][3] = a[0][0]*b[0][3]+a[0][1]*b[1][3]+a[0][2]*b[2][3]+a[0][3]*b[3][3];\
00905                                                 \
00906    c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0]+a[1][3]*b[3][0];\
00907    c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1]+a[1][3]*b[3][1];\
00908    c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2]+a[1][3]*b[3][2];\
00909    c[1][3] = a[1][0]*b[0][3]+a[1][1]*b[1][3]+a[1][2]*b[2][3]+a[1][3]*b[3][3];\
00910                                                 \
00911    c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0]+a[2][3]*b[3][0];\
00912    c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1]+a[2][3]*b[3][1];\
00913    c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2]+a[2][3]*b[3][2];\
00914    c[2][3] = a[2][0]*b[0][3]+a[2][1]*b[1][3]+a[2][2]*b[2][3]+a[2][3]*b[3][3];\
00915                                                 \
00916    c[3][0] = a[3][0]*b[0][0]+a[3][1]*b[1][0]+a[3][2]*b[2][0]+a[3][3]*b[3][0];\
00917    c[3][1] = a[3][0]*b[0][1]+a[3][1]*b[1][1]+a[3][2]*b[2][1]+a[3][3]*b[3][1];\
00918    c[3][2] = a[3][0]*b[0][2]+a[3][1]*b[1][2]+a[3][2]*b[2][2]+a[3][3]*b[3][2];\
00919    c[3][3] = a[3][0]*b[0][3]+a[3][1]*b[1][3]+a[3][2]*b[2][3]+a[3][3]*b[3][3];\
00920 }\
00921 
00922 
00924 #define MAT_DOT_VEC_2X2(p,m,v)                                  \
00925 {                                                               \
00926    p[0] = m[0][0]*v[0] + m[0][1]*v[1];                          \
00927    p[1] = m[1][0]*v[0] + m[1][1]*v[1];                          \
00928 }\
00929 
00930 
00932 #define MAT_DOT_VEC_3X3(p,m,v)                                  \
00933 {                                                               \
00934    p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];           \
00935    p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];           \
00936    p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];           \
00937 }\
00938 
00939 
00943 #define MAT_DOT_VEC_4X4(p,m,v)                                  \
00944 {                                                               \
00945    p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3]*v[3];    \
00946    p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3]*v[3];    \
00947    p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3]*v[3];    \
00948    p[3] = m[3][0]*v[0] + m[3][1]*v[1] + m[3][2]*v[2] + m[3][3]*v[3];    \
00949 }\
00950 
00951 
00956 #define MAT_DOT_VEC_3X4(p,m,v)                                  \
00957 {                                                               \
00958    p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3]; \
00959    p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3]; \
00960    p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3]; \
00961 }\
00962 
00963 
00966 #define VEC_DOT_MAT_3X3(p,v,m)                                  \
00967 {                                                               \
00968    p[0] = v[0]*m[0][0] + v[1]*m[1][0] + v[2]*m[2][0];           \
00969    p[1] = v[0]*m[0][1] + v[1]*m[1][1] + v[2]*m[2][1];           \
00970    p[2] = v[0]*m[0][2] + v[1]*m[1][2] + v[2]*m[2][2];           \
00971 }\
00972 
00973 
00977 #define MAT_DOT_VEC_2X3(p,m,v)                                  \
00978 {                                                               \
00979    p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2];                \
00980    p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2];                \
00981 }\
00982 
00983 
00984 #define MAT_TRANSFORM_PLANE_4X4(pout,m,plane)\
00985 {                                                               \
00986    pout[0] = m[0][0]*plane[0] + m[0][1]*plane[1]  + m[0][2]*plane[2];\
00987    pout[1] = m[1][0]*plane[0] + m[1][1]*plane[1]  + m[1][2]*plane[2];\
00988    pout[2] = m[2][0]*plane[0] + m[2][1]*plane[1]  + m[2][2]*plane[2];\
00989    pout[3] = m[0][3]*pout[0] + m[1][3]*pout[1]  + m[2][3]*pout[2] + plane[3];\
00990 }\
00991 
00992 
00993 
01003 #define INV_TRANSP_MAT_DOT_VEC_2X2(p,m,v)                       \
01004 {                                                               \
01005    GREAL det;                                           \
01006                                                                 \
01007    det = m[0][0]*m[1][1] - m[0][1]*m[1][0];                     \
01008    p[0] = m[1][1]*v[0] - m[1][0]*v[1];                          \
01009    p[1] = - m[0][1]*v[0] + m[0][0]*v[1];                        \
01010                                                                 \
01011    /* if matrix not singular, and not orthonormal, then renormalize */ \
01012    if ((det!=1.0f) && (det != 0.0f)) {                          \
01013       det = 1.0f / det;                                         \
01014       p[0] *= det;                                              \
01015       p[1] *= det;                                              \
01016    }                                                            \
01017 }\
01018 
01019 
01027 #define NORM_XFORM_2X2(p,m,v)                                   \
01028 {                                                               \
01029    GREAL len;                                                   \
01030                                                                 \
01031    /* do nothing if off-diagonals are zero and diagonals are    \
01032     * equal */                                                  \
01033    if ((m[0][1] != 0.0) || (m[1][0] != 0.0) || (m[0][0] != m[1][1])) { \
01034       p[0] = m[1][1]*v[0] - m[1][0]*v[1];                       \
01035       p[1] = - m[0][1]*v[0] + m[0][0]*v[1];                     \
01036                                                                 \
01037       len = p[0]*p[0] + p[1]*p[1];                              \
01038       GIM_INV_SQRT(len,len);                                    \
01039       p[0] *= len;                                              \
01040       p[1] *= len;                                              \
01041    } else {                                                     \
01042       VEC_COPY_2 (p, v);                                        \
01043    }                                                            \
01044 }\
01045 
01046 
01052 #define OUTER_PRODUCT_2X2(m,v,t)                                \
01053 {                                                               \
01054    m[0][0] = v[0] * t[0];                                       \
01055    m[0][1] = v[0] * t[1];                                       \
01056                                                                 \
01057    m[1][0] = v[1] * t[0];                                       \
01058    m[1][1] = v[1] * t[1];                                       \
01059 }\
01060 
01061 
01067 #define OUTER_PRODUCT_3X3(m,v,t)                                \
01068 {                                                               \
01069    m[0][0] = v[0] * t[0];                                       \
01070    m[0][1] = v[0] * t[1];                                       \
01071    m[0][2] = v[0] * t[2];                                       \
01072                                                                 \
01073    m[1][0] = v[1] * t[0];                                       \
01074    m[1][1] = v[1] * t[1];                                       \
01075    m[1][2] = v[1] * t[2];                                       \
01076                                                                 \
01077    m[2][0] = v[2] * t[0];                                       \
01078    m[2][1] = v[2] * t[1];                                       \
01079    m[2][2] = v[2] * t[2];                                       \
01080 }\
01081 
01082 
01088 #define OUTER_PRODUCT_4X4(m,v,t)                                \
01089 {                                                               \
01090    m[0][0] = v[0] * t[0];                                       \
01091    m[0][1] = v[0] * t[1];                                       \
01092    m[0][2] = v[0] * t[2];                                       \
01093    m[0][3] = v[0] * t[3];                                       \
01094                                                                 \
01095    m[1][0] = v[1] * t[0];                                       \
01096    m[1][1] = v[1] * t[1];                                       \
01097    m[1][2] = v[1] * t[2];                                       \
01098    m[1][3] = v[1] * t[3];                                       \
01099                                                                 \
01100    m[2][0] = v[2] * t[0];                                       \
01101    m[2][1] = v[2] * t[1];                                       \
01102    m[2][2] = v[2] * t[2];                                       \
01103    m[2][3] = v[2] * t[3];                                       \
01104                                                                 \
01105    m[3][0] = v[3] * t[0];                                       \
01106    m[3][1] = v[3] * t[1];                                       \
01107    m[3][2] = v[3] * t[2];                                       \
01108    m[3][3] = v[3] * t[3];                                       \
01109 }\
01110 
01111 
01117 #define ACCUM_OUTER_PRODUCT_2X2(m,v,t)                          \
01118 {                                                               \
01119    m[0][0] += v[0] * t[0];                                      \
01120    m[0][1] += v[0] * t[1];                                      \
01121                                                                 \
01122    m[1][0] += v[1] * t[0];                                      \
01123    m[1][1] += v[1] * t[1];                                      \
01124 }\
01125 
01126 
01132 #define ACCUM_OUTER_PRODUCT_3X3(m,v,t)                          \
01133 {                                                               \
01134    m[0][0] += v[0] * t[0];                                      \
01135    m[0][1] += v[0] * t[1];                                      \
01136    m[0][2] += v[0] * t[2];                                      \
01137                                                                 \
01138    m[1][0] += v[1] * t[0];                                      \
01139    m[1][1] += v[1] * t[1];                                      \
01140    m[1][2] += v[1] * t[2];                                      \
01141                                                                 \
01142    m[2][0] += v[2] * t[0];                                      \
01143    m[2][1] += v[2] * t[1];                                      \
01144    m[2][2] += v[2] * t[2];                                      \
01145 }\
01146 
01147 
01153 #define ACCUM_OUTER_PRODUCT_4X4(m,v,t)                          \
01154 {                                                               \
01155    m[0][0] += v[0] * t[0];                                      \
01156    m[0][1] += v[0] * t[1];                                      \
01157    m[0][2] += v[0] * t[2];                                      \
01158    m[0][3] += v[0] * t[3];                                      \
01159                                                                 \
01160    m[1][0] += v[1] * t[0];                                      \
01161    m[1][1] += v[1] * t[1];                                      \
01162    m[1][2] += v[1] * t[2];                                      \
01163    m[1][3] += v[1] * t[3];                                      \
01164                                                                 \
01165    m[2][0] += v[2] * t[0];                                      \
01166    m[2][1] += v[2] * t[1];                                      \
01167    m[2][2] += v[2] * t[2];                                      \
01168    m[2][3] += v[2] * t[3];                                      \
01169                                                                 \
01170    m[3][0] += v[3] * t[0];                                      \
01171    m[3][1] += v[3] * t[1];                                      \
01172    m[3][2] += v[3] * t[2];                                      \
01173    m[3][3] += v[3] * t[3];                                      \
01174 }\
01175 
01176 
01181 #define DETERMINANT_2X2(d,m)                                    \
01182 {                                                               \
01183    d = m[0][0] * m[1][1] - m[0][1] * m[1][0];                   \
01184 }\
01185 
01186 
01191 #define DETERMINANT_3X3(d,m)                                    \
01192 {                                                               \
01193    d = m[0][0] * (m[1][1]*m[2][2] - m[1][2] * m[2][1]);         \
01194    d -= m[0][1] * (m[1][0]*m[2][2] - m[1][2] * m[2][0]);        \
01195    d += m[0][2] * (m[1][0]*m[2][1] - m[1][1] * m[2][0]);        \
01196 }\
01197 
01198 
01202 #define COFACTOR_4X4_IJ(fac,m,i,j)                              \
01203 {                                                               \
01204    GUINT __ii[4], __jj[4], __k;                                         \
01205                                                                 \
01206    for (__k=0; __k<i; __k++) __ii[__k] = __k;                           \
01207    for (__k=i; __k<3; __k++) __ii[__k] = __k+1;                         \
01208    for (__k=0; __k<j; __k++) __jj[__k] = __k;                           \
01209    for (__k=j; __k<3; __k++) __jj[__k] = __k+1;                         \
01210                                                                 \
01211    (fac) = m[__ii[0]][__jj[0]] * (m[__ii[1]][__jj[1]]*m[__ii[2]][__jj[2]]       \
01212                             - m[__ii[1]][__jj[2]]*m[__ii[2]][__jj[1]]); \
01213    (fac) -= m[__ii[0]][__jj[1]] * (m[__ii[1]][__jj[0]]*m[__ii[2]][__jj[2]]      \
01214                              - m[__ii[1]][__jj[2]]*m[__ii[2]][__jj[0]]);\
01215    (fac) += m[__ii[0]][__jj[2]] * (m[__ii[1]][__jj[0]]*m[__ii[2]][__jj[1]]      \
01216                              - m[__ii[1]][__jj[1]]*m[__ii[2]][__jj[0]]);\
01217                                                                 \
01218    __k = i+j;                                                   \
01219    if ( __k != (__k/2)*2) {                                             \
01220       (fac) = -(fac);                                           \
01221    }                                                            \
01222 }\
01223 
01224 
01229 #define DETERMINANT_4X4(d,m)                                    \
01230 {                                                               \
01231    GREAL cofac;                                         \
01232    COFACTOR_4X4_IJ (cofac, m, 0, 0);                            \
01233    d = m[0][0] * cofac;                                         \
01234    COFACTOR_4X4_IJ (cofac, m, 0, 1);                            \
01235    d += m[0][1] * cofac;                                        \
01236    COFACTOR_4X4_IJ (cofac, m, 0, 2);                            \
01237    d += m[0][2] * cofac;                                        \
01238    COFACTOR_4X4_IJ (cofac, m, 0, 3);                            \
01239    d += m[0][3] * cofac;                                        \
01240 }\
01241 
01242 
01247 #define COFACTOR_2X2(a,m)                                       \
01248 {                                                               \
01249    a[0][0] = (m)[1][1];                                         \
01250    a[0][1] = - (m)[1][0];                                               \
01251    a[1][0] = - (m)[0][1];                                               \
01252    a[1][1] = (m)[0][0];                                         \
01253 }\
01254 
01255 
01260 #define COFACTOR_3X3(a,m)                                       \
01261 {                                                               \
01262    a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1];                 \
01263    a[0][1] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]);             \
01264    a[0][2] = m[1][0]*m[2][1] - m[1][1]*m[2][0];                 \
01265    a[1][0] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]);             \
01266    a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0];                 \
01267    a[1][2] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]);             \
01268    a[2][0] = m[0][1]*m[1][2] - m[0][2]*m[1][1];                 \
01269    a[2][1] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]);             \
01270    a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]);                \
01271 }\
01272 
01273 
01278 #define COFACTOR_4X4(a,m)                                       \
01279 {                                                               \
01280    int i,j;                                                     \
01281                                                                 \
01282    for (i=0; i<4; i++) {                                        \
01283       for (j=0; j<4; j++) {                                     \
01284          COFACTOR_4X4_IJ (a[i][j], m, i, j);                    \
01285       }                                                         \
01286    }                                                            \
01287 }\
01288 
01289 
01295 #define ADJOINT_2X2(a,m)                                        \
01296 {                                                               \
01297    a[0][0] = (m)[1][1];                                         \
01298    a[1][0] = - (m)[1][0];                                               \
01299    a[0][1] = - (m)[0][1];                                               \
01300    a[1][1] = (m)[0][0];                                         \
01301 }\
01302 
01303 
01309 #define ADJOINT_3X3(a,m)                                        \
01310 {                                                               \
01311    a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1];                 \
01312    a[1][0] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]);             \
01313    a[2][0] = m[1][0]*m[2][1] - m[1][1]*m[2][0];                 \
01314    a[0][1] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]);             \
01315    a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0];                 \
01316    a[2][1] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]);             \
01317    a[0][2] = m[0][1]*m[1][2] - m[0][2]*m[1][1];                 \
01318    a[1][2] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]);             \
01319    a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]);                \
01320 }\
01321 
01322 
01328 #define ADJOINT_4X4(a,m)                                        \
01329 {                                                               \
01330    char _i_,_j_;                                                        \
01331                                                                 \
01332    for (_i_=0; _i_<4; _i_++) {                                  \
01333       for (_j_=0; _j_<4; _j_++) {                                       \
01334          COFACTOR_4X4_IJ (a[_j_][_i_], m, _i_, _j_);                    \
01335       }                                                         \
01336    }                                                            \
01337 }\
01338 
01339 
01344 #define SCALE_ADJOINT_2X2(a,s,m)                                \
01345 {                                                               \
01346    a[0][0] = (s) * m[1][1];                                     \
01347    a[1][0] = - (s) * m[1][0];                                   \
01348    a[0][1] = - (s) * m[0][1];                                   \
01349    a[1][1] = (s) * m[0][0];                                     \
01350 }\
01351 
01352 
01357 #define SCALE_ADJOINT_3X3(a,s,m)                                \
01358 {                                                               \
01359    a[0][0] = (s) * (m[1][1] * m[2][2] - m[1][2] * m[2][1]);     \
01360    a[1][0] = (s) * (m[1][2] * m[2][0] - m[1][0] * m[2][2]);     \
01361    a[2][0] = (s) * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);     \
01362                                                                 \
01363    a[0][1] = (s) * (m[0][2] * m[2][1] - m[0][1] * m[2][2]);     \
01364    a[1][1] = (s) * (m[0][0] * m[2][2] - m[0][2] * m[2][0]);     \
01365    a[2][1] = (s) * (m[0][1] * m[2][0] - m[0][0] * m[2][1]);     \
01366                                                                 \
01367    a[0][2] = (s) * (m[0][1] * m[1][2] - m[0][2] * m[1][1]);     \
01368    a[1][2] = (s) * (m[0][2] * m[1][0] - m[0][0] * m[1][2]);     \
01369    a[2][2] = (s) * (m[0][0] * m[1][1] - m[0][1] * m[1][0]);     \
01370 }\
01371 
01372 
01377 #define SCALE_ADJOINT_4X4(a,s,m)                                \
01378 {                                                               \
01379    char _i_,_j_; \
01380    for (_i_=0; _i_<4; _i_++) {                                  \
01381       for (_j_=0; _j_<4; _j_++) {                                       \
01382          COFACTOR_4X4_IJ (a[_j_][_i_], m, _i_, _j_);                    \
01383          a[_j_][_i_] *= s;                                              \
01384       }                                                         \
01385    }                                                            \
01386 }\
01387 
01388 
01393 #define INVERT_2X2(b,det,a)                     \
01394 {                                               \
01395    GREAL _tmp_;                                 \
01396    DETERMINANT_2X2 (det, a);                    \
01397    _tmp_ = 1.0 / (det);                         \
01398    SCALE_ADJOINT_2X2 (b, _tmp_, a);             \
01399 }\
01400 
01401 
01407 #define INVERT_3X3(b,det,a)                     \
01408 {                                               \
01409    GREAL _tmp_;                                 \
01410    DETERMINANT_3X3 (det, a);                    \
01411    _tmp_ = 1.0 / (det);                         \
01412    SCALE_ADJOINT_3X3 (b, _tmp_, a);             \
01413 }\
01414 
01415 
01421 #define INVERT_4X4(b,det,a)                     \
01422 {                                               \
01423    GREAL _tmp_;                                 \
01424    DETERMINANT_4X4 (det, a);                    \
01425    _tmp_ = 1.0 / (det);                         \
01426    SCALE_ADJOINT_4X4 (b, _tmp_, a);             \
01427 }\
01428 
01429 
01430 #define MAT_GET_ROW(mat,vec3,rowindex)\
01431 {\
01432     vec3[0] = mat[rowindex][0];\
01433     vec3[1] = mat[rowindex][1];\
01434     vec3[2] = mat[rowindex][2]; \
01435 }\
01436 
01437 
01438 #define MAT_GET_ROW2(mat,vec2,rowindex)\
01439 {\
01440     vec2[0] = mat[rowindex][0];\
01441     vec2[1] = mat[rowindex][1];\
01442 }\
01443 
01444 
01446 #define MAT_GET_ROW4(mat,vec4,rowindex)\
01447 {\
01448     vec4[0] = mat[rowindex][0];\
01449     vec4[1] = mat[rowindex][1];\
01450     vec4[2] = mat[rowindex][2];\
01451     vec4[3] = mat[rowindex][3];\
01452 }\
01453 
01454 
01455 #define MAT_GET_COL(mat,vec3,colindex)\
01456 {\
01457     vec3[0] = mat[0][colindex];\
01458     vec3[1] = mat[1][colindex];\
01459     vec3[2] = mat[2][colindex]; \
01460 }\
01461 
01462 
01463 #define MAT_GET_COL2(mat,vec2,colindex)\
01464 {\
01465     vec2[0] = mat[0][colindex];\
01466     vec2[1] = mat[1][colindex];\
01467 }\
01468 
01469 
01471 #define MAT_GET_COL4(mat,vec4,colindex)\
01472 {\
01473     vec4[0] = mat[0][colindex];\
01474     vec4[1] = mat[1][colindex];\
01475     vec4[2] = mat[2][colindex];\
01476     vec4[3] = mat[3][colindex];\
01477 }\
01478 
01479 
01480 #define MAT_GET_X(mat,vec3)\
01481 {\
01482     MAT_GET_COL(mat,vec3,0);\
01483 }\
01484 
01485 
01486 #define MAT_GET_Y(mat,vec3)\
01487 {\
01488     MAT_GET_COL(mat,vec3,1);\
01489 }\
01490 
01491 
01492 #define MAT_GET_Z(mat,vec3)\
01493 {\
01494     MAT_GET_COL(mat,vec3,2);\
01495 }\
01496 
01497 
01499 #define MAT_SET_X(mat,vec3)\
01500 {\
01501     mat[0][0] = vec3[0];\
01502     mat[1][0] = vec3[1];\
01503     mat[2][0] = vec3[2];\
01504 }\
01505 
01506 
01507 #define MAT_SET_Y(mat,vec3)\
01508 {\
01509     mat[0][1] = vec3[0];\
01510     mat[1][1] = vec3[1];\
01511     mat[2][1] = vec3[2];\
01512 }\
01513 
01514 
01515 #define MAT_SET_Z(mat,vec3)\
01516 {\
01517     mat[0][2] = vec3[0];\
01518     mat[1][2] = vec3[1];\
01519     mat[2][2] = vec3[2];\
01520 }\
01521 
01522 
01524 #define MAT_GET_TRANSLATION(mat,vec3)\
01525 {\
01526     vec3[0] = mat[0][3];\
01527     vec3[1] = mat[1][3];\
01528     vec3[2] = mat[2][3]; \
01529 }\
01530 
01531 
01532 #define MAT_SET_TRANSLATION(mat,vec3)\
01533 {\
01534     mat[0][3] = vec3[0];\
01535     mat[1][3] = vec3[1];\
01536     mat[2][3] = vec3[2]; \
01537 }\
01538 
01539 
01540 
01542 #define MAT_DOT_ROW(mat,vec3,rowindex) (vec3[0]*mat[rowindex][0] + vec3[1]*mat[rowindex][1] + vec3[2]*mat[rowindex][2])
01543 
01545 #define MAT_DOT_ROW2(mat,vec2,rowindex) (vec2[0]*mat[rowindex][0] + vec2[1]*mat[rowindex][1])
01546 
01548 #define MAT_DOT_ROW4(mat,vec4,rowindex) (vec4[0]*mat[rowindex][0] + vec4[1]*mat[rowindex][1] + vec4[2]*mat[rowindex][2] + vec4[3]*mat[rowindex][3])
01549 
01550 
01552 #define MAT_DOT_COL(mat,vec3,colindex) (vec3[0]*mat[0][colindex] + vec3[1]*mat[1][colindex] + vec3[2]*mat[2][colindex])
01553 
01555 #define MAT_DOT_COL2(mat,vec2,colindex) (vec2[0]*mat[0][colindex] + vec2[1]*mat[1][colindex])
01556 
01558 #define MAT_DOT_COL4(mat,vec4,colindex) (vec4[0]*mat[0][colindex] + vec4[1]*mat[1][colindex] + vec4[2]*mat[2][colindex] + vec4[3]*mat[3][colindex])
01559 
01564 #define INV_MAT_DOT_VEC_3X3(p,m,v)                                      \
01565 {                                                               \
01566    p[0] = MAT_DOT_COL(m,v,0); \
01567    p[1] = MAT_DOT_COL(m,v,1);   \
01568    p[2] = MAT_DOT_COL(m,v,2);   \
01569 }\
01570 
01571 
01572 
01573 #endif // GIM_VECTOR_H_INCLUDED
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


bullet
Author(s): Erwin Coumans, ROS package maintained by Tully Foote
autogenerated on Wed Oct 31 2012 07:54:31