src/BulletCollision/Gimpact/gim_linear_math.h

   1 #ifndef GIM_LINEAR_H_INCLUDED
   2 #define GIM_LINEAR_H_INCLUDED
   3
   4 /*! \file gim_linear_math.h
   5 *\author Francisco Leon Najera
   6 Type Independant Vector and matrix operations.
   7 */
   8 /*
   9 -----------------------------------------------------------------------------
  10 This source file is part of GIMPACT Library.
  11
  12 For the latest info, see http://gimpact.sourceforge.net/
  13
  14 Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
  15 email: projectileman@yahoo.com
  16
  17  This library is free software; you can redistribute it and/or
  18  modify it under the terms of EITHER:
  19    (1) The GNU Lesser General Public License as published by the Free
  20        Software Foundation; either version 2.1 of the License, or (at
  21        your option) any later version. The text of the GNU Lesser
  22        General Public License is included with this library in the
  23        file GIMPACT-LICENSE-LGPL.TXT.
  24    (2) The BSD-style license that is included with this library in
  25        the file GIMPACT-LICENSE-BSD.TXT.
  26    (3) The zlib/libpng license that is included with this library in
  27        the file GIMPACT-LICENSE-ZLIB.TXT.
  28
  29  This library is distributed in the hope that it will be useful,
  30  but WITHOUT ANY WARRANTY; without even the implied warranty of
  31  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
  32  GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
  33
  34 -----------------------------------------------------------------------------
  35 */
  36
  37
  38 #include "gim_math.h"
  39 #include "gim_geom_types.h"
  40
  41
  42
  43
  44 //! Zero out a 2D vector
  45 #define VEC_ZERO_2(a)                           \
  46 {                                               \
  47    (a)[0] = (a)[1] = 0.0f;                      \
  48 }\
  49
  50
  51 //! Zero out a 3D vector
  52 #define VEC_ZERO(a)                             \
  53 {                                               \
  54    (a)[0] = (a)[1] = (a)[2] = 0.0f;             \
  55 }\
  56
  57
  58 /// Zero out a 4D vector
  59 #define VEC_ZERO_4(a)                           \
  60 {                                               \
  61    (a)[0] = (a)[1] = (a)[2] = (a)[3] = 0.0f;    \
  62 }\
  63
  64
  65 /// Vector copy
  66 #define VEC_COPY_2(b,a)                         \
  67 {                                               \
  68    (b)[0] = (a)[0];                             \
  69    (b)[1] = (a)[1];                             \
  70 }\
  71
  72
  73 /// Copy 3D vector
  74 #define VEC_COPY(b,a)                           \
  75 {                                               \
  76    (b)[0] = (a)[0];                             \
  77    (b)[1] = (a)[1];                             \
  78    (b)[2] = (a)[2];                             \
  79 }\
  80
  81
  82 /// Copy 4D vector
  83 #define VEC_COPY_4(b,a)                         \
  84 {                                               \
  85    (b)[0] = (a)[0];                             \
  86    (b)[1] = (a)[1];                             \
  87    (b)[2] = (a)[2];                             \
  88    (b)[3] = (a)[3];                             \
  89 }\
  90
  91 /// VECTOR SWAP
  92 #define VEC_SWAP(b,a)                           \
  93 {  \
  94     GIM_SWAP_NUMBERS((b)[0],(a)[0]);\
  95     GIM_SWAP_NUMBERS((b)[1],(a)[1]);\
  96     GIM_SWAP_NUMBERS((b)[2],(a)[2]);\
  97 }\
  98
  99 /// Vector difference
 100 #define VEC_DIFF_2(v21,v2,v1)                   \
 101 {                                               \
 102    (v21)[0] = (v2)[0] - (v1)[0];                \
 103    (v21)[1] = (v2)[1] - (v1)[1];                \
 104 }\
 105
 106
 107 /// Vector difference
 108 #define VEC_DIFF(v21,v2,v1)                     \
 109 {                                               \
 110    (v21)[0] = (v2)[0] - (v1)[0];                \
 111    (v21)[1] = (v2)[1] - (v1)[1];                \
 112    (v21)[2] = (v2)[2] - (v1)[2];                \
 113 }\
 114
 115
 116 /// Vector difference
 117 #define VEC_DIFF_4(v21,v2,v1)                   \
 118 {                                               \
 119    (v21)[0] = (v2)[0] - (v1)[0];                \
 120    (v21)[1] = (v2)[1] - (v1)[1];                \
 121    (v21)[2] = (v2)[2] - (v1)[2];                \
 122    (v21)[3] = (v2)[3] - (v1)[3];                \
 123 }\
 124
 125
 126 /// Vector sum
 127 #define VEC_SUM_2(v21,v2,v1)                    \
 128 {                                               \
 129    (v21)[0] = (v2)[0] + (v1)[0];                \
 130    (v21)[1] = (v2)[1] + (v1)[1];                \
 131 }\
 132
 133
 134 /// Vector sum
 135 #define VEC_SUM(v21,v2,v1)                      \
 136 {                                               \
 137    (v21)[0] = (v2)[0] + (v1)[0];                \
 138    (v21)[1] = (v2)[1] + (v1)[1];                \
 139    (v21)[2] = (v2)[2] + (v1)[2];                \
 140 }\
 141
 142
 143 /// Vector sum
 144 #define VEC_SUM_4(v21,v2,v1)                    \
 145 {                                               \
 146    (v21)[0] = (v2)[0] + (v1)[0];                \
 147    (v21)[1] = (v2)[1] + (v1)[1];                \
 148    (v21)[2] = (v2)[2] + (v1)[2];                \
 149    (v21)[3] = (v2)[3] + (v1)[3];                \
 150 }\
 151
 152
 153 /// scalar times vector
 154 #define VEC_SCALE_2(c,a,b)                      \
 155 {                                               \
 156    (c)[0] = (a)*(b)[0];                         \
 157    (c)[1] = (a)*(b)[1];                         \
 158 }\
 159
 160
 161 /// scalar times vector
 162 #define VEC_SCALE(c,a,b)                        \
 163 {                                               \
 164    (c)[0] = (a)*(b)[0];                         \
 165    (c)[1] = (a)*(b)[1];                         \
 166    (c)[2] = (a)*(b)[2];                         \
 167 }\
 168
 169
 170 /// scalar times vector
 171 #define VEC_SCALE_4(c,a,b)                      \
 172 {                                               \
 173    (c)[0] = (a)*(b)[0];                         \
 174    (c)[1] = (a)*(b)[1];                         \
 175    (c)[2] = (a)*(b)[2];                         \
 176    (c)[3] = (a)*(b)[3];                         \
 177 }\
 178
 179
 180 /// accumulate scaled vector
 181 #define VEC_ACCUM_2(c,a,b)                      \
 182 {                                               \
 183    (c)[0] += (a)*(b)[0];                        \
 184    (c)[1] += (a)*(b)[1];                        \
 185 }\
 186
 187
 188 /// accumulate scaled vector
 189 #define VEC_ACCUM(c,a,b)                        \
 190 {                                               \
 191    (c)[0] += (a)*(b)[0];                        \
 192    (c)[1] += (a)*(b)[1];                        \
 193    (c)[2] += (a)*(b)[2];                        \
 194 }\
 195
 196
 197 /// accumulate scaled vector
 198 #define VEC_ACCUM_4(c,a,b)                      \
 199 {                                               \
 200    (c)[0] += (a)*(b)[0];                        \
 201    (c)[1] += (a)*(b)[1];                        \
 202    (c)[2] += (a)*(b)[2];                        \
 203    (c)[3] += (a)*(b)[3];                        \
 204 }\
 205
 206
 207 /// Vector dot product
 208 #define VEC_DOT_2(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1])
 209
 210
 211 /// Vector dot product
 212 #define VEC_DOT(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2])
 213
 214 /// Vector dot product
 215 #define VEC_DOT_4(a,b)  ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2] + (a)[3]*(b)[3])
 216
 217 /// vector impact parameter (squared)
 218 #define VEC_IMPACT_SQ(bsq,direction,position) {\
 219    GREAL _llel_ = VEC_DOT(direction, position);\
 220    bsq = VEC_DOT(position, position) - _llel_*_llel_;\
 221 }\
 222
 223
 224 /// vector impact parameter
 225 #define VEC_IMPACT(bsq,direction,position)      {\
 226    VEC_IMPACT_SQ(bsq,direction,position);               \
 227    GIM_SQRT(bsq,bsq);                                   \
 228 }\
 229
 230 /// Vector length
 231 #define VEC_LENGTH_2(a,l)\
 232 {\
 233     GREAL _pp = VEC_DOT_2(a,a);\
 234     GIM_SQRT(_pp,l);\
 235 }\
 236
 237
 238 /// Vector length
 239 #define VEC_LENGTH(a,l)\
 240 {\
 241     GREAL _pp = VEC_DOT(a,a);\
 242     GIM_SQRT(_pp,l);\
 243 }\
 244
 245
 246 /// Vector length
 247 #define VEC_LENGTH_4(a,l)\
 248 {\
 249     GREAL _pp = VEC_DOT_4(a,a);\
 250     GIM_SQRT(_pp,l);\
 251 }\
 252
 253 /// Vector inv length
 254 #define VEC_INV_LENGTH_2(a,l)\
 255 {\
 256     GREAL _pp = VEC_DOT_2(a,a);\
 257     GIM_INV_SQRT(_pp,l);\
 258 }\
 259
 260
 261 /// Vector inv length
 262 #define VEC_INV_LENGTH(a,l)\
 263 {\
 264     GREAL _pp = VEC_DOT(a,a);\
 265     GIM_INV_SQRT(_pp,l);\
 266 }\
 267
 268
 269 /// Vector inv length
 270 #define VEC_INV_LENGTH_4(a,l)\
 271 {\
 272     GREAL _pp = VEC_DOT_4(a,a);\
 273     GIM_INV_SQRT(_pp,l);\
 274 }\
 275
 276
 277
 278 /// distance between two points
 279 #define VEC_DISTANCE(_len,_va,_vb) {\
 280     vec3f _tmp_;                                \
 281     VEC_DIFF(_tmp_, _vb, _va);                  \
 282     VEC_LENGTH(_tmp_,_len);                     \
 283 }\
 284
 285
 286 /// Vector length
 287 #define VEC_CONJUGATE_LENGTH(a,l)\
 288 {\
 289     GREAL _pp = 1.0 - a[0]*a[0] - a[1]*a[1] - a[2]*a[2];\
 290     GIM_SQRT(_pp,l);\
 291 }\
 292
 293
 294 /// Vector length
 295 #define VEC_NORMALIZE(a) {      \
 296     GREAL len;\
 297     VEC_INV_LENGTH(a,len); \
 298     if(len<G_REAL_INFINITY)\
 299     {\
 300         a[0] *= len;                            \
 301         a[1] *= len;                            \
 302         a[2] *= len;                            \
 303     }                                           \
 304 }\
 305
 306 /// Set Vector size
 307 #define VEC_RENORMALIZE(a,newlen) {     \
 308     GREAL len;\
 309     VEC_INV_LENGTH(a,len); \
 310     if(len<G_REAL_INFINITY)\
 311     {\
 312         len *= newlen;\
 313         a[0] *= len;                            \
 314         a[1] *= len;                            \
 315         a[2] *= len;                            \
 316     }                                           \
 317 }\
 318
 319 /// Vector cross
 320 #define VEC_CROSS(c,a,b)                \
 321 {                                               \
 322    c[0] = (a)[1] * (b)[2] - (a)[2] * (b)[1];    \
 323    c[1] = (a)[2] * (b)[0] - (a)[0] * (b)[2];    \
 324    c[2] = (a)[0] * (b)[1] - (a)[1] * (b)[0];    \
 325 }\
 326
 327
 328 /*! Vector perp -- assumes that n is of unit length
 329  * accepts vector v, subtracts out any component parallel to n */
 330 #define VEC_PERPENDICULAR(vp,v,n)                       \
 331 {                                               \
 332    GREAL dot = VEC_DOT(v, n);                   \
 333    vp[0] = (v)[0] - dot*(n)[0];         \
 334    vp[1] = (v)[1] - dot*(n)[1];         \
 335    vp[2] = (v)[2] - dot*(n)[2];         \
 336 }\
 337
 338
 339 /*! Vector parallel -- assumes that n is of unit length */
 340 #define VEC_PARALLEL(vp,v,n)                    \
 341 {                                               \
 342    GREAL dot = VEC_DOT(v, n);                   \
 343    vp[0] = (dot) * (n)[0];                      \
 344    vp[1] = (dot) * (n)[1];                      \
 345    vp[2] = (dot) * (n)[2];                      \
 346 }\
 347
 348 /*! Same as Vector parallel --  n can have any length
 349  * accepts vector v, subtracts out any component perpendicular to n */
 350 #define VEC_PROJECT(vp,v,n)                     \
 351 { \
 352         GREAL scalar = VEC_DOT(v, n);                   \
 353         scalar/= VEC_DOT(n, n); \
 354         vp[0] = (scalar) * (n)[0];                      \
 355     vp[1] = (scalar) * (n)[1];                  \
 356     vp[2] = (scalar) * (n)[2];                  \
 357 }\
 358
 359
 360 /*! accepts vector v*/
 361 #define VEC_UNPROJECT(vp,v,n)                   \
 362 { \
 363         GREAL scalar = VEC_DOT(v, n);                   \
 364         scalar = VEC_DOT(n, n)/scalar; \
 365         vp[0] = (scalar) * (n)[0];                      \
 366     vp[1] = (scalar) * (n)[1];                  \
 367     vp[2] = (scalar) * (n)[2];                  \
 368 }\
 369
 370
 371 /*! Vector reflection -- assumes n is of unit length
 372  Takes vector v, reflects it against reflector n, and returns vr */
 373 #define VEC_REFLECT(vr,v,n)                     \
 374 {                                               \
 375    GREAL dot = VEC_DOT(v, n);                   \
 376    vr[0] = (v)[0] - 2.0 * (dot) * (n)[0];       \
 377    vr[1] = (v)[1] - 2.0 * (dot) * (n)[1];       \
 378    vr[2] = (v)[2] - 2.0 * (dot) * (n)[2];       \
 379 }\
 380
 381
 382 /*! Vector blending
 383 Takes two vectors a, b, blends them together with two scalars */
 384 #define VEC_BLEND_AB(vr,sa,a,sb,b)                      \
 385 {                                               \
 386    vr[0] = (sa) * (a)[0] + (sb) * (b)[0];       \
 387    vr[1] = (sa) * (a)[1] + (sb) * (b)[1];       \
 388    vr[2] = (sa) * (a)[2] + (sb) * (b)[2];       \
 389 }\
 390
 391 /*! Vector blending
 392 Takes two vectors a, b, blends them together with s <=1 */
 393 #define VEC_BLEND(vr,a,b,s) VEC_BLEND_AB(vr,(1-s),a,s,b)
 394
 395 #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];
 396
 397 //! Finds the bigger cartesian coordinate from a vector
 398 #define VEC_MAYOR_COORD(vec, maxc)\
 399 {\
 400         GREAL A[] = {fabs(vec[0]),fabs(vec[1]),fabs(vec[2])};\
 401     maxc =  A[0]>A[1]?(A[0]>A[2]?0:2):(A[1]>A[2]?1:2);\
 402 }\
 403
 404 //! Finds the 2 smallest cartesian coordinates from a vector
 405 #define VEC_MINOR_AXES(vec, i0, i1)\
 406 {\
 407         VEC_MAYOR_COORD(vec,i0);\
 408         i0 = (i0+1)%3;\
 409         i1 = (i0+1)%3;\
 410 }\
 411
 412
 413
 414
 415 #define VEC_EQUAL(v1,v2) (v1[0]==v2[0]&&v1[1]==v2[1]&&v1[2]==v2[2])
 416
 417 #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]))
 418
 419
 420 /// Vector cross
 421 #define X_AXIS_CROSS_VEC(dst,src)\
 422 {                                          \
 423         dst[0] = 0.0f;     \
 424         dst[1] = -src[2];  \
 425         dst[2] = src[1];  \
 426 }\
 427
 428 #define Y_AXIS_CROSS_VEC(dst,src)\
 429 {                                          \
 430         dst[0] = src[2];     \
 431         dst[1] = 0.0f;  \
 432         dst[2] = -src[0];  \
 433 }\
 434
 435 #define Z_AXIS_CROSS_VEC(dst,src)\
 436 {                                          \
 437         dst[0] = -src[1];     \
 438         dst[1] = src[0];  \
 439         dst[2] = 0.0f;  \
 440 }\
 441
 442
 443
 444
 445
 446
 447 /// initialize matrix
 448 #define IDENTIFY_MATRIX_3X3(m)                  \
 449 {                                               \
 450    m[0][0] = 1.0;                               \
 451    m[0][1] = 0.0;                               \
 452    m[0][2] = 0.0;                               \
 453                                                 \
 454    m[1][0] = 0.0;                               \
 455    m[1][1] = 1.0;                               \
 456    m[1][2] = 0.0;                               \
 457                                                 \
 458    m[2][0] = 0.0;                               \
 459    m[2][1] = 0.0;                               \
 460    m[2][2] = 1.0;                               \
 461 }\
 462
 463 /*! initialize matrix */
 464 #define IDENTIFY_MATRIX_4X4(m)                  \
 465 {                                               \
 466    m[0][0] = 1.0;                               \
 467    m[0][1] = 0.0;                               \
 468    m[0][2] = 0.0;                               \
 469    m[0][3] = 0.0;                               \
 470                                                 \
 471    m[1][0] = 0.0;                               \
 472    m[1][1] = 1.0;                               \
 473    m[1][2] = 0.0;                               \
 474    m[1][3] = 0.0;                               \
 475                                                 \
 476    m[2][0] = 0.0;                               \
 477    m[2][1] = 0.0;                               \
 478    m[2][2] = 1.0;                               \
 479    m[2][3] = 0.0;                               \
 480                                                 \
 481    m[3][0] = 0.0;                               \
 482    m[3][1] = 0.0;                               \
 483    m[3][2] = 0.0;                               \
 484    m[3][3] = 1.0;                               \
 485 }\
 486
 487 /*! initialize matrix */
 488 #define ZERO_MATRIX_4X4(m)                      \
 489 {                                               \
 490    m[0][0] = 0.0;                               \
 491    m[0][1] = 0.0;                               \
 492    m[0][2] = 0.0;                               \
 493    m[0][3] = 0.0;                               \
 494                                                 \
 495    m[1][0] = 0.0;                               \
 496    m[1][1] = 0.0;                               \
 497    m[1][2] = 0.0;                               \
 498    m[1][3] = 0.0;                               \
 499                                                 \
 500    m[2][0] = 0.0;                               \
 501    m[2][1] = 0.0;                               \
 502    m[2][2] = 0.0;                               \
 503    m[2][3] = 0.0;                               \
 504                                                 \
 505    m[3][0] = 0.0;                               \
 506    m[3][1] = 0.0;                               \
 507    m[3][2] = 0.0;                               \
 508    m[3][3] = 0.0;                               \
 509 }\
 510
 511 /*! matrix rotation  X */
 512 #define ROTX_CS(m,cosine,sine)          \
 513 {                                       \
 514    /* rotation about the x-axis */      \
 515                                         \
 516    m[0][0] = 1.0;                       \
 517    m[0][1] = 0.0;                       \
 518    m[0][2] = 0.0;                       \
 519    m[0][3] = 0.0;                       \
 520                                         \
 521    m[1][0] = 0.0;                       \
 522    m[1][1] = (cosine);                  \
 523    m[1][2] = (sine);                    \
 524    m[1][3] = 0.0;                       \
 525                                         \
 526    m[2][0] = 0.0;                       \
 527    m[2][1] = -(sine);                   \
 528    m[2][2] = (cosine);                  \
 529    m[2][3] = 0.0;                       \
 530                                         \
 531    m[3][0] = 0.0;                       \
 532    m[3][1] = 0.0;                       \
 533    m[3][2] = 0.0;                       \
 534    m[3][3] = 1.0;                       \
 535 }\
 536
 537 /*! matrix rotation  Y */
 538 #define ROTY_CS(m,cosine,sine)          \
 539 {                                       \
 540    /* rotation about the y-axis */      \
 541                                         \
 542    m[0][0] = (cosine);                  \
 543    m[0][1] = 0.0;                       \
 544    m[0][2] = -(sine);                   \
 545    m[0][3] = 0.0;                       \
 546                                         \
 547    m[1][0] = 0.0;                       \
 548    m[1][1] = 1.0;                       \
 549    m[1][2] = 0.0;                       \
 550    m[1][3] = 0.0;                       \
 551                                         \
 552    m[2][0] = (sine);                    \
 553    m[2][1] = 0.0;                       \
 554    m[2][2] = (cosine);                  \
 555    m[2][3] = 0.0;                       \
 556                                         \
 557    m[3][0] = 0.0;                       \
 558    m[3][1] = 0.0;                       \
 559    m[3][2] = 0.0;                       \
 560    m[3][3] = 1.0;                       \
 561 }\
 562
 563 /*! matrix rotation  Z */
 564 #define ROTZ_CS(m,cosine,sine)          \
 565 {                                       \
 566    /* rotation about the z-axis */      \
 567                                         \
 568    m[0][0] = (cosine);                  \
 569    m[0][1] = (sine);                    \
 570    m[0][2] = 0.0;                       \
 571    m[0][3] = 0.0;                       \
 572                                         \
 573    m[1][0] = -(sine);                   \
 574    m[1][1] = (cosine);                  \
 575    m[1][2] = 0.0;                       \
 576    m[1][3] = 0.0;                       \
 577                                         \
 578    m[2][0] = 0.0;                       \
 579    m[2][1] = 0.0;                       \
 580    m[2][2] = 1.0;                       \
 581    m[2][3] = 0.0;                       \
 582                                         \
 583    m[3][0] = 0.0;                       \
 584    m[3][1] = 0.0;                       \
 585    m[3][2] = 0.0;                       \
 586    m[3][3] = 1.0;                       \
 587 }\
 588
 589 /*! matrix copy */
 590 #define COPY_MATRIX_2X2(b,a)    \
 591 {                               \
 592    b[0][0] = a[0][0];           \
 593    b[0][1] = a[0][1];           \
 594                                 \
 595    b[1][0] = a[1][0];           \
 596    b[1][1] = a[1][1];           \
 597                                 \
 598 }\
 599
 600
 601 /*! matrix copy */
 602 #define COPY_MATRIX_2X3(b,a)    \
 603 {                               \
 604    b[0][0] = a[0][0];           \
 605    b[0][1] = a[0][1];           \
 606    b[0][2] = a[0][2];           \
 607                                 \
 608    b[1][0] = a[1][0];           \
 609    b[1][1] = a[1][1];           \
 610    b[1][2] = a[1][2];           \
 611 }\
 612
 613
 614 /*! matrix copy */
 615 #define COPY_MATRIX_3X3(b,a)    \
 616 {                               \
 617    b[0][0] = a[0][0];           \
 618    b[0][1] = a[0][1];           \
 619    b[0][2] = a[0][2];           \
 620                                 \
 621    b[1][0] = a[1][0];           \
 622    b[1][1] = a[1][1];           \
 623    b[1][2] = a[1][2];           \
 624                                 \
 625    b[2][0] = a[2][0];           \
 626    b[2][1] = a[2][1];           \
 627    b[2][2] = a[2][2];           \
 628 }\
 629
 630
 631 /*! matrix copy */
 632 #define COPY_MATRIX_4X4(b,a)    \
 633 {                               \
 634    b[0][0] = a[0][0];           \
 635    b[0][1] = a[0][1];           \
 636    b[0][2] = a[0][2];           \
 637    b[0][3] = a[0][3];           \
 638                                 \
 639    b[1][0] = a[1][0];           \
 640    b[1][1] = a[1][1];           \
 641    b[1][2] = a[1][2];           \
 642    b[1][3] = a[1][3];           \
 643                                 \
 644    b[2][0] = a[2][0];           \
 645    b[2][1] = a[2][1];           \
 646    b[2][2] = a[2][2];           \
 647    b[2][3] = a[2][3];           \
 648                                 \
 649    b[3][0] = a[3][0];           \
 650    b[3][1] = a[3][1];           \
 651    b[3][2] = a[3][2];           \
 652    b[3][3] = a[3][3];           \
 653 }\
 654
 655
 656 /*! matrix transpose */
 657 #define TRANSPOSE_MATRIX_2X2(b,a)       \
 658 {                               \
 659    b[0][0] = a[0][0];           \
 660    b[0][1] = a[1][0];           \
 661                                 \
 662    b[1][0] = a[0][1];           \
 663    b[1][1] = a[1][1];           \
 664 }\
 665
 666
 667 /*! matrix transpose */
 668 #define TRANSPOSE_MATRIX_3X3(b,a)       \
 669 {                               \
 670    b[0][0] = a[0][0];           \
 671    b[0][1] = a[1][0];           \
 672    b[0][2] = a[2][0];           \
 673                                 \
 674    b[1][0] = a[0][1];           \
 675    b[1][1] = a[1][1];           \
 676    b[1][2] = a[2][1];           \
 677                                 \
 678    b[2][0] = a[0][2];           \
 679    b[2][1] = a[1][2];           \
 680    b[2][2] = a[2][2];           \
 681 }\
 682
 683
 684 /*! matrix transpose */
 685 #define TRANSPOSE_MATRIX_4X4(b,a)       \
 686 {                               \
 687    b[0][0] = a[0][0];           \
 688    b[0][1] = a[1][0];           \
 689    b[0][2] = a[2][0];           \
 690    b[0][3] = a[3][0];           \
 691                                 \
 692    b[1][0] = a[0][1];           \
 693    b[1][1] = a[1][1];           \
 694    b[1][2] = a[2][1];           \
 695    b[1][3] = a[3][1];           \
 696                                 \
 697    b[2][0] = a[0][2];           \
 698    b[2][1] = a[1][2];           \
 699    b[2][2] = a[2][2];           \
 700    b[2][3] = a[3][2];           \
 701                                 \
 702    b[3][0] = a[0][3];           \
 703    b[3][1] = a[1][3];           \
 704    b[3][2] = a[2][3];           \
 705    b[3][3] = a[3][3];           \
 706 }\
 707
 708
 709 /*! multiply matrix by scalar */
 710 #define SCALE_MATRIX_2X2(b,s,a)         \
 711 {                                       \
 712    b[0][0] = (s) * a[0][0];             \
 713    b[0][1] = (s) * a[0][1];             \
 714                                         \
 715    b[1][0] = (s) * a[1][0];             \
 716    b[1][1] = (s) * a[1][1];             \
 717 }\
 718
 719
 720 /*! multiply matrix by scalar */
 721 #define SCALE_MATRIX_3X3(b,s,a)         \
 722 {                                       \
 723    b[0][0] = (s) * a[0][0];             \
 724    b[0][1] = (s) * a[0][1];             \
 725    b[0][2] = (s) * a[0][2];             \
 726                                         \
 727    b[1][0] = (s) * a[1][0];             \
 728    b[1][1] = (s) * a[1][1];             \
 729    b[1][2] = (s) * a[1][2];             \
 730                                         \
 731    b[2][0] = (s) * a[2][0];             \
 732    b[2][1] = (s) * a[2][1];             \
 733    b[2][2] = (s) * a[2][2];             \
 734 }\
 735
 736
 737 /*! multiply matrix by scalar */
 738 #define SCALE_MATRIX_4X4(b,s,a)         \
 739 {                                       \
 740    b[0][0] = (s) * a[0][0];             \
 741    b[0][1] = (s) * a[0][1];             \
 742    b[0][2] = (s) * a[0][2];             \
 743    b[0][3] = (s) * a[0][3];             \
 744                                         \
 745    b[1][0] = (s) * a[1][0];             \
 746    b[1][1] = (s) * a[1][1];             \
 747    b[1][2] = (s) * a[1][2];             \
 748    b[1][3] = (s) * a[1][3];             \
 749                                         \
 750    b[2][0] = (s) * a[2][0];             \
 751    b[2][1] = (s) * a[2][1];             \
 752    b[2][2] = (s) * a[2][2];             \
 753    b[2][3] = (s) * a[2][3];             \
 754                                         \
 755    b[3][0] = s * a[3][0];               \
 756    b[3][1] = s * a[3][1];               \
 757    b[3][2] = s * a[3][2];               \
 758    b[3][3] = s * a[3][3];               \
 759 }\
 760
 761
 762 /*! multiply matrix by scalar */
 763 #define SCALE_VEC_MATRIX_2X2(b,svec,a)          \
 764 {                                       \
 765    b[0][0] = svec[0] * a[0][0];         \
 766    b[1][0] = svec[0] * a[1][0];         \
 767                                         \
 768    b[0][1] = svec[1] * a[0][1];         \
 769    b[1][1] = svec[1] * a[1][1];         \
 770 }\
 771
 772
 773 /*! multiply matrix by scalar. Each columns is scaled by each scalar vector component */
 774 #define SCALE_VEC_MATRIX_3X3(b,svec,a)          \
 775 {                                       \
 776    b[0][0] = svec[0] * a[0][0];         \
 777    b[1][0] = svec[0] * a[1][0];         \
 778    b[2][0] = svec[0] * a[2][0];         \
 779                                         \
 780    b[0][1] = svec[1] * a[0][1];         \
 781    b[1][1] = svec[1] * a[1][1];         \
 782    b[2][1] = svec[1] * a[2][1];         \
 783                                         \
 784    b[0][2] = svec[2] * a[0][2];         \
 785    b[1][2] = svec[2] * a[1][2];         \
 786    b[2][2] = svec[2] * a[2][2];         \
 787 }\
 788
 789
 790 /*! multiply matrix by scalar */
 791 #define SCALE_VEC_MATRIX_4X4(b,svec,a)          \
 792 {                                       \
 793    b[0][0] = svec[0] * a[0][0];         \
 794    b[1][0] = svec[0] * a[1][0];         \
 795    b[2][0] = svec[0] * a[2][0];         \
 796    b[3][0] = svec[0] * a[3][0];         \
 797                                         \
 798    b[0][1] = svec[1] * a[0][1];         \
 799    b[1][1] = svec[1] * a[1][1];         \
 800    b[2][1] = svec[1] * a[2][1];         \
 801    b[3][1] = svec[1] * a[3][1];         \
 802                                         \
 803    b[0][2] = svec[2] * a[0][2];         \
 804    b[1][2] = svec[2] * a[1][2];         \
 805    b[2][2] = svec[2] * a[2][2];         \
 806    b[3][2] = svec[2] * a[3][2];         \
 807    \
 808    b[0][3] = svec[3] * a[0][3];         \
 809    b[1][3] = svec[3] * a[1][3];         \
 810    b[2][3] = svec[3] * a[2][3];         \
 811    b[3][3] = svec[3] * a[3][3];         \
 812 }\
 813
 814
 815 /*! multiply matrix by scalar */
 816 #define ACCUM_SCALE_MATRIX_2X2(b,s,a)           \
 817 {                                       \
 818    b[0][0] += (s) * a[0][0];            \
 819    b[0][1] += (s) * a[0][1];            \
 820                                         \
 821    b[1][0] += (s) * a[1][0];            \
 822    b[1][1] += (s) * a[1][1];            \
 823 }\
 824
 825
 826 /*! multiply matrix by scalar */
 827 #define ACCUM_SCALE_MATRIX_3X3(b,s,a)           \
 828 {                                       \
 829    b[0][0] += (s) * a[0][0];            \
 830    b[0][1] += (s) * a[0][1];            \
 831    b[0][2] += (s) * a[0][2];            \
 832                                         \
 833    b[1][0] += (s) * a[1][0];            \
 834    b[1][1] += (s) * a[1][1];            \
 835    b[1][2] += (s) * a[1][2];            \
 836                                         \
 837    b[2][0] += (s) * a[2][0];            \
 838    b[2][1] += (s) * a[2][1];            \
 839    b[2][2] += (s) * a[2][2];            \
 840 }\
 841
 842
 843 /*! multiply matrix by scalar */
 844 #define ACCUM_SCALE_MATRIX_4X4(b,s,a)           \
 845 {                                       \
 846    b[0][0] += (s) * a[0][0];            \
 847    b[0][1] += (s) * a[0][1];            \
 848    b[0][2] += (s) * a[0][2];            \
 849    b[0][3] += (s) * a[0][3];            \
 850                                         \
 851    b[1][0] += (s) * a[1][0];            \
 852    b[1][1] += (s) * a[1][1];            \
 853    b[1][2] += (s) * a[1][2];            \
 854    b[1][3] += (s) * a[1][3];            \
 855                                         \
 856    b[2][0] += (s) * a[2][0];            \
 857    b[2][1] += (s) * a[2][1];            \
 858    b[2][2] += (s) * a[2][2];            \
 859    b[2][3] += (s) * a[2][3];            \
 860                                         \
 861    b[3][0] += (s) * a[3][0];            \
 862    b[3][1] += (s) * a[3][1];            \
 863    b[3][2] += (s) * a[3][2];            \
 864    b[3][3] += (s) * a[3][3];            \
 865 }\
 866
 867 /*! matrix product */
 868 /*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/
 869 #define MATRIX_PRODUCT_2X2(c,a,b)               \
 870 {                                               \
 871    c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0];   \
 872    c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1];   \
 873                                                 \
 874    c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0];   \
 875    c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1];   \
 876                                                 \
 877 }\
 878
 879 /*! matrix product */
 880 /*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/
 881 #define MATRIX_PRODUCT_3X3(c,a,b)                               \
 882 {                                                               \
 883    c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0];   \
 884    c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1];   \
 885    c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2];   \
 886                                                                 \
 887    c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0];   \
 888    c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1];   \
 889    c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2];   \
 890                                                                 \
 891    c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0];   \
 892    c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1];   \
 893    c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2];   \
 894 }\
 895
 896
 897 /*! matrix product */
 898 /*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/
 899 #define MATRIX_PRODUCT_4X4(c,a,b)               \
 900 {                                               \
 901    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];\
 902    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];\
 903    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];\
 904    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];\
 905                                                 \
 906    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];\
 907    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];\
 908    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];\
 909    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];\
 910                                                 \
 911    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];\
 912    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];\
 913    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];\
 914    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];\
 915                                                 \
 916    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];\
 917    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];\
 918    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];\
 919    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];\
 920 }\
 921
 922
 923 /*! matrix times vector */
 924 #define MAT_DOT_VEC_2X2(p,m,v)                                  \
 925 {                                                               \
 926    p[0] = m[0][0]*v[0] + m[0][1]*v[1];                          \
 927    p[1] = m[1][0]*v[0] + m[1][1]*v[1];                          \
 928 }\
 929
 930
 931 /*! matrix times vector */
 932 #define MAT_DOT_VEC_3X3(p,m,v)                                  \
 933 {                                                               \
 934    p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];           \
 935    p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];           \
 936    p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];           \
 937 }\
 938
 939
 940 /*! matrix times vector
 941 v is a vec4f
 942 */
 943 #define MAT_DOT_VEC_4X4(p,m,v)                                  \
 944 {                                                               \
 945    p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3]*v[3];    \
 946    p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3]*v[3];    \
 947    p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3]*v[3];    \
 948    p[3] = m[3][0]*v[0] + m[3][1]*v[1] + m[3][2]*v[2] + m[3][3]*v[3];    \
 949 }\
 950
 951 /*! matrix times vector
 952 v is a vec3f
 953 and m is a mat4f<br>
 954 Last column is added as the position
 955 */
 956 #define MAT_DOT_VEC_3X4(p,m,v)                                  \
 957 {                                                               \
 958    p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3]; \
 959    p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3]; \
 960    p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3]; \
 961 }\
 962
 963
 964 /*! vector transpose times matrix */
 965 /*! p[j] = v[0]*m[0][j] + v[1]*m[1][j] + v[2]*m[2][j]; */
 966 #define VEC_DOT_MAT_3X3(p,v,m)                                  \
 967 {                                                               \
 968    p[0] = v[0]*m[0][0] + v[1]*m[1][0] + v[2]*m[2][0];           \
 969    p[1] = v[0]*m[0][1] + v[1]*m[1][1] + v[2]*m[2][1];           \
 970    p[2] = v[0]*m[0][2] + v[1]*m[1][2] + v[2]*m[2][2];           \
 971 }\
 972
 973
 974 /*! affine matrix times vector */
 975 /** The matrix is assumed to be an affine matrix, with last two
 976  * entries representing a translation */
 977 #define MAT_DOT_VEC_2X3(p,m,v)                                  \
 978 {                                                               \
 979    p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2];                \
 980    p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2];                \
 981 }\
 982
 983 //! Transform a plane
 984 #define MAT_TRANSFORM_PLANE_4X4(pout,m,plane)\
 985 {                                                               \
 986    pout[0] = m[0][0]*plane[0] + m[0][1]*plane[1]  + m[0][2]*plane[2];\
 987    pout[1] = m[1][0]*plane[0] + m[1][1]*plane[1]  + m[1][2]*plane[2];\
 988    pout[2] = m[2][0]*plane[0] + m[2][1]*plane[1]  + m[2][2]*plane[2];\
 989    pout[3] = m[0][3]*pout[0] + m[1][3]*pout[1]  + m[2][3]*pout[2] + plane[3];\
 990 }\
 991
 992
 993
 994 /** inverse transpose of matrix times vector
 995  *
 996  * This macro computes inverse transpose of matrix m,
 997  * and multiplies vector v into it, to yeild vector p
 998  *
 999  * DANGER !!! Do Not use this on normal vectors!!!
1000  * It will leave normals the wrong length !!!
1001  * See macro below for use on normals.
1002  */
1003 #define INV_TRANSP_MAT_DOT_VEC_2X2(p,m,v)                       \
1004 {                                                               \
1005    GREAL det;                                           \
1006                                                                 \
1007    det = m[0][0]*m[1][1] - m[0][1]*m[1][0];                     \
1008    p[0] = m[1][1]*v[0] - m[1][0]*v[1];                          \
1009    p[1] = - m[0][1]*v[0] + m[0][0]*v[1];                        \
1010                                                                 \
1011    /* if matrix not singular, and not orthonormal, then renormalize */ \
1012    if ((det!=1.0f) && (det != 0.0f)) {                          \
1013       det = 1.0f / det;                                         \
1014       p[0] *= det;                                              \
1015       p[1] *= det;                                              \
1016    }                                                            \
1017 }\
1018
1019
1020 /** transform normal vector by inverse transpose of matrix
1021  * and then renormalize the vector
1022  *
1023  * This macro computes inverse transpose of matrix m,
1024  * and multiplies vector v into it, to yeild vector p
1025  * Vector p is then normalized.
1026  */
1027 #define NORM_XFORM_2X2(p,m,v)                                   \
1028 {                                                               \
1029    GREAL len;                                                   \
1030                                                                 \
1031    /* do nothing if off-diagonals are zero and diagonals are    \
1032     * equal */                                                  \
1033    if ((m[0][1] != 0.0) || (m[1][0] != 0.0) || (m[0][0] != m[1][1])) { \
1034       p[0] = m[1][1]*v[0] - m[1][0]*v[1];                       \
1035       p[1] = - m[0][1]*v[0] + m[0][0]*v[1];                     \
1036                                                                 \
1037       len = p[0]*p[0] + p[1]*p[1];                              \
1038       GIM_INV_SQRT(len,len);                                    \
1039       p[0] *= len;                                              \
1040       p[1] *= len;                                              \
1041    } else {                                                     \
1042       VEC_COPY_2 (p, v);                                        \
1043    }                                                            \
1044 }\
1045
1046
1047 /** outer product of vector times vector transpose
1048  *
1049  * The outer product of vector v and vector transpose t yeilds
1050  * dyadic matrix m.
1051  */
1052 #define OUTER_PRODUCT_2X2(m,v,t)                                \
1053 {                                                               \
1054    m[0][0] = v[0] * t[0];                                       \
1055    m[0][1] = v[0] * t[1];                                       \
1056                                                                 \
1057    m[1][0] = v[1] * t[0];                                       \
1058    m[1][1] = v[1] * t[1];                                       \
1059 }\
1060
1061
1062 /** outer product of vector times vector transpose
1063  *
1064  * The outer product of vector v and vector transpose t yeilds
1065  * dyadic matrix m.
1066  */
1067 #define OUTER_PRODUCT_3X3(m,v,t)                                \
1068 {                                                               \
1069    m[0][0] = v[0] * t[0];                                       \
1070    m[0][1] = v[0] * t[1];                                       \
1071    m[0][2] = v[0] * t[2];                                       \
1072                                                                 \
1073    m[1][0] = v[1] * t[0];                                       \
1074    m[1][1] = v[1] * t[1];                                       \
1075    m[1][2] = v[1] * t[2];                                       \
1076                                                                 \
1077    m[2][0] = v[2] * t[0];                                       \
1078    m[2][1] = v[2] * t[1];                                       \
1079    m[2][2] = v[2] * t[2];                                       \
1080 }\
1081
1082
1083 /** outer product of vector times vector transpose
1084  *
1085  * The outer product of vector v and vector transpose t yeilds
1086  * dyadic matrix m.
1087  */
1088 #define OUTER_PRODUCT_4X4(m,v,t)                                \
1089 {                                                               \
1090    m[0][0] = v[0] * t[0];                                       \
1091    m[0][1] = v[0] * t[1];                                       \
1092    m[0][2] = v[0] * t[2];                                       \
1093    m[0][3] = v[0] * t[3];                                       \
1094                                                                 \
1095    m[1][0] = v[1] * t[0];                                       \
1096    m[1][1] = v[1] * t[1];                                       \
1097    m[1][2] = v[1] * t[2];                                       \
1098    m[1][3] = v[1] * t[3];                                       \
1099                                                                 \
1100    m[2][0] = v[2] * t[0];                                       \
1101    m[2][1] = v[2] * t[1];                                       \
1102    m[2][2] = v[2] * t[2];                                       \
1103    m[2][3] = v[2] * t[3];                                       \
1104                                                                 \
1105    m[3][0] = v[3] * t[0];                                       \
1106    m[3][1] = v[3] * t[1];                                       \
1107    m[3][2] = v[3] * t[2];                                       \
1108    m[3][3] = v[3] * t[3];                                       \
1109 }\
1110
1111
1112 /** outer product of vector times vector transpose
1113  *
1114  * The outer product of vector v and vector transpose t yeilds
1115  * dyadic matrix m.
1116  */
1117 #define ACCUM_OUTER_PRODUCT_2X2(m,v,t)                          \
1118 {                                                               \
1119    m[0][0] += v[0] * t[0];                                      \
1120    m[0][1] += v[0] * t[1];                                      \
1121                                                                 \
1122    m[1][0] += v[1] * t[0];                                      \
1123    m[1][1] += v[1] * t[1];                                      \
1124 }\
1125
1126
1127 /** outer product of vector times vector transpose
1128  *
1129  * The outer product of vector v and vector transpose t yeilds
1130  * dyadic matrix m.
1131  */
1132 #define ACCUM_OUTER_PRODUCT_3X3(m,v,t)                          \
1133 {                                                               \
1134    m[0][0] += v[0] * t[0];                                      \
1135    m[0][1] += v[0] * t[1];                                      \
1136    m[0][2] += v[0] * t[2];                                      \
1137                                                                 \
1138    m[1][0] += v[1] * t[0];                                      \
1139    m[1][1] += v[1] * t[1];                                      \
1140    m[1][2] += v[1] * t[2];                                      \
1141                                                                 \
1142    m[2][0] += v[2] * t[0];                                      \
1143    m[2][1] += v[2] * t[1];                                      \
1144    m[2][2] += v[2] * t[2];                                      \
1145 }\
1146
1147
1148 /** outer product of vector times vector transpose
1149  *
1150  * The outer product of vector v and vector transpose t yeilds
1151  * dyadic matrix m.
1152  */
1153 #define ACCUM_OUTER_PRODUCT_4X4(m,v,t)                          \
1154 {                                                               \
1155    m[0][0] += v[0] * t[0];                                      \
1156    m[0][1] += v[0] * t[1];                                      \
1157    m[0][2] += v[0] * t[2];                                      \
1158    m[0][3] += v[0] * t[3];                                      \
1159                                                                 \
1160    m[1][0] += v[1] * t[0];                                      \
1161    m[1][1] += v[1] * t[1];                                      \
1162    m[1][2] += v[1] * t[2];                                      \
1163    m[1][3] += v[1] * t[3];                                      \
1164                                                                 \
1165    m[2][0] += v[2] * t[0];                                      \
1166    m[2][1] += v[2] * t[1];                                      \
1167    m[2][2] += v[2] * t[2];                                      \
1168    m[2][3] += v[2] * t[3];                                      \
1169                                                                 \
1170    m[3][0] += v[3] * t[0];                                      \
1171    m[3][1] += v[3] * t[1];                                      \
1172    m[3][2] += v[3] * t[2];                                      \
1173    m[3][3] += v[3] * t[3];                                      \
1174 }\
1175
1176
1177 /** determinant of matrix
1178  *
1179  * Computes determinant of matrix m, returning d
1180  */
1181 #define DETERMINANT_2X2(d,m)                                    \
1182 {                                                               \
1183    d = m[0][0] * m[1][1] - m[0][1] * m[1][0];                   \
1184 }\
1185
1186
1187 /** determinant of matrix
1188  *
1189  * Computes determinant of matrix m, returning d
1190  */
1191 #define DETERMINANT_3X3(d,m)                                    \
1192 {                                                               \
1193    d = m[0][0] * (m[1][1]*m[2][2] - m[1][2] * m[2][1]);         \
1194    d -= m[0][1] * (m[1][0]*m[2][2] - m[1][2] * m[2][0]);        \
1195    d += m[0][2] * (m[1][0]*m[2][1] - m[1][1] * m[2][0]);        \
1196 }\
1197
1198
1199 /** i,j,th cofactor of a 4x4 matrix
1200  *
1201  */
1202 #define COFACTOR_4X4_IJ(fac,m,i,j)                              \
1203 {                                                               \
1204    GUINT __ii[4], __jj[4], __k;                                         \
1205                                                                 \
1206    for (__k=0; __k<i; __k++) __ii[__k] = __k;                           \
1207    for (__k=i; __k<3; __k++) __ii[__k] = __k+1;                         \
1208    for (__k=0; __k<j; __k++) __jj[__k] = __k;                           \
1209    for (__k=j; __k<3; __k++) __jj[__k] = __k+1;                         \
1210                                                                 \
1211    (fac) = m[__ii[0]][__jj[0]] * (m[__ii[1]][__jj[1]]*m[__ii[2]][__jj[2]]       \
1212                             - m[__ii[1]][__jj[2]]*m[__ii[2]][__jj[1]]); \
1213    (fac) -= m[__ii[0]][__jj[1]] * (m[__ii[1]][__jj[0]]*m[__ii[2]][__jj[2]]      \
1214                              - m[__ii[1]][__jj[2]]*m[__ii[2]][__jj[0]]);\
1215    (fac) += m[__ii[0]][__jj[2]] * (m[__ii[1]][__jj[0]]*m[__ii[2]][__jj[1]]      \
1216                              - m[__ii[1]][__jj[1]]*m[__ii[2]][__jj[0]]);\
1217                                                                 \
1218    __k = i+j;                                                   \
1219    if ( __k != (__k/2)*2) {                                             \
1220       (fac) = -(fac);                                           \
1221    }                                                            \
1222 }\
1223
1224
1225 /** determinant of matrix
1226  *
1227  * Computes determinant of matrix m, returning d
1228  */
1229 #define DETERMINANT_4X4(d,m)                                    \
1230 {                                                               \
1231    GREAL cofac;                                         \
1232    COFACTOR_4X4_IJ (cofac, m, 0, 0);                            \
1233    d = m[0][0] * cofac;                                         \
1234    COFACTOR_4X4_IJ (cofac, m, 0, 1);                            \
1235    d += m[0][1] * cofac;                                        \
1236    COFACTOR_4X4_IJ (cofac, m, 0, 2);                            \
1237    d += m[0][2] * cofac;                                        \
1238    COFACTOR_4X4_IJ (cofac, m, 0, 3);                            \
1239    d += m[0][3] * cofac;                                        \
1240 }\
1241
1242
1243 /** cofactor of matrix
1244  *
1245  * Computes cofactor of matrix m, returning a
1246  */
1247 #define COFACTOR_2X2(a,m)                                       \
1248 {                                                               \
1249    a[0][0] = (m)[1][1];                                         \
1250    a[0][1] = - (m)[1][0];                                               \
1251    a[1][0] = - (m)[0][1];                                               \
1252    a[1][1] = (m)[0][0];                                         \
1253 }\
1254
1255
1256 /** cofactor of matrix
1257  *
1258  * Computes cofactor of matrix m, returning a
1259  */
1260 #define COFACTOR_3X3(a,m)                                       \
1261 {                                                               \
1262    a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1];                 \
1263    a[0][1] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]);             \
1264    a[0][2] = m[1][0]*m[2][1] - m[1][1]*m[2][0];                 \
1265    a[1][0] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]);             \
1266    a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0];                 \
1267    a[1][2] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]);             \
1268    a[2][0] = m[0][1]*m[1][2] - m[0][2]*m[1][1];                 \
1269    a[2][1] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]);             \
1270    a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]);                \
1271 }\
1272
1273
1274 /** cofactor of matrix
1275  *
1276  * Computes cofactor of matrix m, returning a
1277  */
1278 #define COFACTOR_4X4(a,m)                                       \
1279 {                                                               \
1280    int i,j;                                                     \
1281                                                                 \
1282    for (i=0; i<4; i++) {                                        \
1283       for (j=0; j<4; j++) {                                     \
1284          COFACTOR_4X4_IJ (a[i][j], m, i, j);                    \
1285       }                                                         \
1286    }                                                            \
1287 }\
1288
1289
1290 /** adjoint of matrix
1291  *
1292  * Computes adjoint of matrix m, returning a
1293  * (Note that adjoint is just the transpose of the cofactor matrix)
1294  */
1295 #define ADJOINT_2X2(a,m)                                        \
1296 {                                                               \
1297    a[0][0] = (m)[1][1];                                         \
1298    a[1][0] = - (m)[1][0];                                               \
1299    a[0][1] = - (m)[0][1];                                               \
1300    a[1][1] = (m)[0][0];                                         \
1301 }\
1302
1303
1304 /** adjoint of matrix
1305  *
1306  * Computes adjoint of matrix m, returning a
1307  * (Note that adjoint is just the transpose of the cofactor matrix)
1308  */
1309 #define ADJOINT_3X3(a,m)                                        \
1310 {                                                               \
1311    a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1];                 \
1312    a[1][0] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]);             \
1313    a[2][0] = m[1][0]*m[2][1] - m[1][1]*m[2][0];                 \
1314    a[0][1] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]);             \
1315    a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0];                 \
1316    a[2][1] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]);             \
1317    a[0][2] = m[0][1]*m[1][2] - m[0][2]*m[1][1];                 \
1318    a[1][2] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]);             \
1319    a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]);                \
1320 }\
1321
1322
1323 /** adjoint of matrix
1324  *
1325  * Computes adjoint of matrix m, returning a
1326  * (Note that adjoint is just the transpose of the cofactor matrix)
1327  */
1328 #define ADJOINT_4X4(a,m)                                        \
1329 {                                                               \
1330    char _i_,_j_;                                                        \
1331                                                                 \
1332    for (_i_=0; _i_<4; _i_++) {                                  \
1333       for (_j_=0; _j_<4; _j_++) {                                       \
1334          COFACTOR_4X4_IJ (a[_j_][_i_], m, _i_, _j_);                    \
1335       }                                                         \
1336    }                                                            \
1337 }\
1338
1339
1340 /** compute adjoint of matrix and scale
1341  *
1342  * Computes adjoint of matrix m, scales it by s, returning a
1343  */
1344 #define SCALE_ADJOINT_2X2(a,s,m)                                \
1345 {                                                               \
1346    a[0][0] = (s) * m[1][1];                                     \
1347    a[1][0] = - (s) * m[1][0];                                   \
1348    a[0][1] = - (s) * m[0][1];                                   \
1349    a[1][1] = (s) * m[0][0];                                     \
1350 }\
1351
1352
1353 /** compute adjoint of matrix and scale
1354  *
1355  * Computes adjoint of matrix m, scales it by s, returning a
1356  */
1357 #define SCALE_ADJOINT_3X3(a,s,m)                                \
1358 {                                                               \
1359    a[0][0] = (s) * (m[1][1] * m[2][2] - m[1][2] * m[2][1]);     \
1360    a[1][0] = (s) * (m[1][2] * m[2][0] - m[1][0] * m[2][2]);     \
1361    a[2][0] = (s) * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);     \
1362                                                                 \
1363    a[0][1] = (s) * (m[0][2] * m[2][1] - m[0][1] * m[2][2]);     \
1364    a[1][1] = (s) * (m[0][0] * m[2][2] - m[0][2] * m[2][0]);     \
1365    a[2][1] = (s) * (m[0][1] * m[2][0] - m[0][0] * m[2][1]);     \
1366                                                                 \
1367    a[0][2] = (s) * (m[0][1] * m[1][2] - m[0][2] * m[1][1]);     \
1368    a[1][2] = (s) * (m[0][2] * m[1][0] - m[0][0] * m[1][2]);     \
1369    a[2][2] = (s) * (m[0][0] * m[1][1] - m[0][1] * m[1][0]);     \
1370 }\
1371
1372
1373 /** compute adjoint of matrix and scale
1374  *
1375  * Computes adjoint of matrix m, scales it by s, returning a
1376  */
1377 #define SCALE_ADJOINT_4X4(a,s,m)                                \
1378 {                                                               \
1379    char _i_,_j_; \
1380    for (_i_=0; _i_<4; _i_++) {                                  \
1381       for (_j_=0; _j_<4; _j_++) {                                       \
1382          COFACTOR_4X4_IJ (a[_j_][_i_], m, _i_, _j_);                    \
1383          a[_j_][_i_] *= s;                                              \
1384       }                                                         \
1385    }                                                            \
1386 }\
1387
1388 /** inverse of matrix
1389  *
1390  * Compute inverse of matrix a, returning determinant m and
1391  * inverse b
1392  */
1393 #define INVERT_2X2(b,det,a)                     \
1394 {                                               \
1395    GREAL _tmp_;                                 \
1396    DETERMINANT_2X2 (det, a);                    \
1397    _tmp_ = 1.0 / (det);                         \
1398    SCALE_ADJOINT_2X2 (b, _tmp_, a);             \
1399 }\
1400
1401
1402 /** inverse of matrix
1403  *
1404  * Compute inverse of matrix a, returning determinant m and
1405  * inverse b
1406  */
1407 #define INVERT_3X3(b,det,a)                     \
1408 {                                               \
1409    GREAL _tmp_;                                 \
1410    DETERMINANT_3X3 (det, a);                    \
1411    _tmp_ = 1.0 / (det);                         \
1412    SCALE_ADJOINT_3X3 (b, _tmp_, a);             \
1413 }\
1414
1415
1416 /** inverse of matrix
1417  *
1418  * Compute inverse of matrix a, returning determinant m and
1419  * inverse b
1420  */
1421 #define INVERT_4X4(b,det,a)                     \
1422 {                                               \
1423    GREAL _tmp_;                                 \
1424    DETERMINANT_4X4 (det, a);                    \
1425    _tmp_ = 1.0 / (det);                         \
1426    SCALE_ADJOINT_4X4 (b, _tmp_, a);             \
1427 }\
1428
1429 //! Get the triple(3) row of a transform matrix
1430 #define MAT_GET_ROW(mat,vec3,rowindex)\
1431 {\
1432     vec3[0] = mat[rowindex][0];\
1433     vec3[1] = mat[rowindex][1];\
1434     vec3[2] = mat[rowindex][2]; \
1435 }\
1436
1437 //! Get the tuple(2) row of a transform matrix
1438 #define MAT_GET_ROW2(mat,vec2,rowindex)\
1439 {\
1440     vec2[0] = mat[rowindex][0];\
1441     vec2[1] = mat[rowindex][1];\
1442 }\
1443
1444
1445 //! Get the quad (4) row of a transform matrix
1446 #define MAT_GET_ROW4(mat,vec4,rowindex)\
1447 {\
1448     vec4[0] = mat[rowindex][0];\
1449     vec4[1] = mat[rowindex][1];\
1450     vec4[2] = mat[rowindex][2];\
1451     vec4[3] = mat[rowindex][3];\
1452 }\
1453
1454 //! Get the triple(3) col of a transform matrix
1455 #define MAT_GET_COL(mat,vec3,colindex)\
1456 {\
1457     vec3[0] = mat[0][colindex];\
1458     vec3[1] = mat[1][colindex];\
1459     vec3[2] = mat[2][colindex]; \
1460 }\
1461
1462 //! Get the tuple(2) col of a transform matrix
1463 #define MAT_GET_COL2(mat,vec2,colindex)\
1464 {\
1465     vec2[0] = mat[0][colindex];\
1466     vec2[1] = mat[1][colindex];\
1467 }\
1468
1469
1470 //! Get the quad (4) col of a transform matrix
1471 #define MAT_GET_COL4(mat,vec4,colindex)\
1472 {\
1473     vec4[0] = mat[0][colindex];\
1474     vec4[1] = mat[1][colindex];\
1475     vec4[2] = mat[2][colindex];\
1476     vec4[3] = mat[3][colindex];\
1477 }\
1478
1479 //! Get the triple(3) col of a transform matrix
1480 #define MAT_GET_X(mat,vec3)\
1481 {\
1482     MAT_GET_COL(mat,vec3,0);\
1483 }\
1484
1485 //! Get the triple(3) col of a transform matrix
1486 #define MAT_GET_Y(mat,vec3)\
1487 {\
1488     MAT_GET_COL(mat,vec3,1);\
1489 }\
1490
1491 //! Get the triple(3) col of a transform matrix
1492 #define MAT_GET_Z(mat,vec3)\
1493 {\
1494     MAT_GET_COL(mat,vec3,2);\
1495 }\
1496
1497
1498 //! Get the triple(3) col of a transform matrix
1499 #define MAT_SET_X(mat,vec3)\
1500 {\
1501     mat[0][0] = vec3[0];\
1502     mat[1][0] = vec3[1];\
1503     mat[2][0] = vec3[2];\
1504 }\
1505
1506 //! Get the triple(3) col of a transform matrix
1507 #define MAT_SET_Y(mat,vec3)\
1508 {\
1509     mat[0][1] = vec3[0];\
1510     mat[1][1] = vec3[1];\
1511     mat[2][1] = vec3[2];\
1512 }\
1513
1514 //! Get the triple(3) col of a transform matrix
1515 #define MAT_SET_Z(mat,vec3)\
1516 {\
1517     mat[0][2] = vec3[0];\
1518     mat[1][2] = vec3[1];\
1519     mat[2][2] = vec3[2];\
1520 }\
1521
1522
1523 //! Get the triple(3) col of a transform matrix
1524 #define MAT_GET_TRANSLATION(mat,vec3)\
1525 {\
1526     vec3[0] = mat[0][3];\
1527     vec3[1] = mat[1][3];\
1528     vec3[2] = mat[2][3]; \
1529 }\
1530
1531 //! Set the triple(3) col of a transform matrix
1532 #define MAT_SET_TRANSLATION(mat,vec3)\
1533 {\
1534     mat[0][3] = vec3[0];\
1535     mat[1][3] = vec3[1];\
1536     mat[2][3] = vec3[2]; \
1537 }\
1538
1539
1540
1541 //! Returns the dot product between a vec3f and the row of a matrix
1542 #define MAT_DOT_ROW(mat,vec3,rowindex) (vec3[0]*mat[rowindex][0] + vec3[1]*mat[rowindex][1] + vec3[2]*mat[rowindex][2])
1543
1544 //! Returns the dot product between a vec2f and the row of a matrix
1545 #define MAT_DOT_ROW2(mat,vec2,rowindex) (vec2[0]*mat[rowindex][0] + vec2[1]*mat[rowindex][1])
1546
1547 //! Returns the dot product between a vec4f and the row of a matrix
1548 #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])
1549
1550
1551 //! Returns the dot product between a vec3f and the col of a matrix
1552 #define MAT_DOT_COL(mat,vec3,colindex) (vec3[0]*mat[0][colindex] + vec3[1]*mat[1][colindex] + vec3[2]*mat[2][colindex])
1553
1554 //! Returns the dot product between a vec2f and the col of a matrix
1555 #define MAT_DOT_COL2(mat,vec2,colindex) (vec2[0]*mat[0][colindex] + vec2[1]*mat[1][colindex])
1556
1557 //! Returns the dot product between a vec4f and the col of a matrix
1558 #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])
1559
1560 /*!Transpose matrix times vector
1561 v is a vec3f
1562 and m is a mat4f<br>
1563 */
1564 #define INV_MAT_DOT_VEC_3X3(p,m,v)                                      \
1565 {                                                               \
1566    p[0] = MAT_DOT_COL(m,v,0); \
1567    p[1] = MAT_DOT_COL(m,v,1);   \
1568    p[2] = MAT_DOT_COL(m,v,2);   \
1569 }\
1570
1571
1572
1573 #endif // GIM_VECTOR_H_INCLUDED