Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/bullet/BulletCollision/Gimpact/gim_linear_math.h @ 2801

Last change on this file since 2801 was 2662, checked in by rgrieder, 16 years ago

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