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