Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/BulletCollision/GIMPACT/gim_linear_math.h @ 1983

Last change on this file since 1983 was 1963, checked in by rgrieder, 16 years ago

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