1
#ifndef GIM_LINEAR_H_INCLUDED
2
#define GIM_LINEAR_H_INCLUDED
4
/*! \file gim_linear_math.h
5
*\author Francisco Leon Najera
6
Type Independant Vector and matrix operations.
9
-----------------------------------------------------------------------------
10
This source file is part of GIMPACT Library.
12
For the latest info, see http://gimpact.sourceforge.net/
14
Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
15
email: projectileman@yahoo.com
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.
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.
34
-----------------------------------------------------------------------------
39
#include "gim_geom_types.h"
44
//! Zero out a 2D vector
45
#define VEC_ZERO_2(a) \
47
(a)[0] = (a)[1] = 0.0f; \
51
//! Zero out a 3D vector
54
(a)[0] = (a)[1] = (a)[2] = 0.0f; \
58
/// Zero out a 4D vector
59
#define VEC_ZERO_4(a) \
61
(a)[0] = (a)[1] = (a)[2] = (a)[3] = 0.0f; \
66
#define VEC_COPY_2(b,a) \
74
#define VEC_COPY(b,a) \
83
#define VEC_COPY_4(b,a) \
92
#define VEC_SWAP(b,a) \
94
GIM_SWAP_NUMBERS((b)[0],(a)[0]);\
95
GIM_SWAP_NUMBERS((b)[1],(a)[1]);\
96
GIM_SWAP_NUMBERS((b)[2],(a)[2]);\
100
#define VEC_DIFF_2(v21,v2,v1) \
102
(v21)[0] = (v2)[0] - (v1)[0]; \
103
(v21)[1] = (v2)[1] - (v1)[1]; \
107
/// Vector difference
108
#define VEC_DIFF(v21,v2,v1) \
110
(v21)[0] = (v2)[0] - (v1)[0]; \
111
(v21)[1] = (v2)[1] - (v1)[1]; \
112
(v21)[2] = (v2)[2] - (v1)[2]; \
116
/// Vector difference
117
#define VEC_DIFF_4(v21,v2,v1) \
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]; \
127
#define VEC_SUM_2(v21,v2,v1) \
129
(v21)[0] = (v2)[0] + (v1)[0]; \
130
(v21)[1] = (v2)[1] + (v1)[1]; \
135
#define VEC_SUM(v21,v2,v1) \
137
(v21)[0] = (v2)[0] + (v1)[0]; \
138
(v21)[1] = (v2)[1] + (v1)[1]; \
139
(v21)[2] = (v2)[2] + (v1)[2]; \
144
#define VEC_SUM_4(v21,v2,v1) \
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]; \
153
/// scalar times vector
154
#define VEC_SCALE_2(c,a,b) \
156
(c)[0] = (a)*(b)[0]; \
157
(c)[1] = (a)*(b)[1]; \
161
/// scalar times vector
162
#define VEC_SCALE(c,a,b) \
164
(c)[0] = (a)*(b)[0]; \
165
(c)[1] = (a)*(b)[1]; \
166
(c)[2] = (a)*(b)[2]; \
170
/// scalar times vector
171
#define VEC_SCALE_4(c,a,b) \
173
(c)[0] = (a)*(b)[0]; \
174
(c)[1] = (a)*(b)[1]; \
175
(c)[2] = (a)*(b)[2]; \
176
(c)[3] = (a)*(b)[3]; \
180
/// accumulate scaled vector
181
#define VEC_ACCUM_2(c,a,b) \
183
(c)[0] += (a)*(b)[0]; \
184
(c)[1] += (a)*(b)[1]; \
188
/// accumulate scaled vector
189
#define VEC_ACCUM(c,a,b) \
191
(c)[0] += (a)*(b)[0]; \
192
(c)[1] += (a)*(b)[1]; \
193
(c)[2] += (a)*(b)[2]; \
197
/// accumulate scaled vector
198
#define VEC_ACCUM_4(c,a,b) \
200
(c)[0] += (a)*(b)[0]; \
201
(c)[1] += (a)*(b)[1]; \
202
(c)[2] += (a)*(b)[2]; \
203
(c)[3] += (a)*(b)[3]; \
207
/// Vector dot product
208
#define VEC_DOT_2(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1])
211
/// Vector dot product
212
#define VEC_DOT(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2])
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])
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_;\
224
/// vector impact parameter
225
#define VEC_IMPACT(bsq,direction,position) {\
226
VEC_IMPACT_SQ(bsq,direction,position); \
231
#define VEC_LENGTH_2(a,l)\
233
GREAL _pp = VEC_DOT_2(a,a);\
239
#define VEC_LENGTH(a,l)\
241
GREAL _pp = VEC_DOT(a,a);\
247
#define VEC_LENGTH_4(a,l)\
249
GREAL _pp = VEC_DOT_4(a,a);\
253
/// Vector inv length
254
#define VEC_INV_LENGTH_2(a,l)\
256
GREAL _pp = VEC_DOT_2(a,a);\
257
GIM_INV_SQRT(_pp,l);\
261
/// Vector inv length
262
#define VEC_INV_LENGTH(a,l)\
264
GREAL _pp = VEC_DOT(a,a);\
265
GIM_INV_SQRT(_pp,l);\
269
/// Vector inv length
270
#define VEC_INV_LENGTH_4(a,l)\
272
GREAL _pp = VEC_DOT_4(a,a);\
273
GIM_INV_SQRT(_pp,l);\
278
/// distance between two points
279
#define VEC_DISTANCE(_len,_va,_vb) {\
281
VEC_DIFF(_tmp_, _vb, _va); \
282
VEC_LENGTH(_tmp_,_len); \
287
#define VEC_CONJUGATE_LENGTH(a,l)\
289
GREAL _pp = 1.0 - a[0]*a[0] - a[1]*a[1] - a[2]*a[2];\
295
#define VEC_NORMALIZE(a) { \
297
VEC_INV_LENGTH(a,len); \
298
if(len<G_REAL_INFINITY)\
307
#define VEC_RENORMALIZE(a,newlen) { \
309
VEC_INV_LENGTH(a,len); \
310
if(len<G_REAL_INFINITY)\
320
#define VEC_CROSS(c,a,b) \
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]; \
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) \
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]; \
339
/*! Vector parallel -- assumes that n is of unit length */
340
#define VEC_PARALLEL(vp,v,n) \
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]; \
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) \
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]; \
360
/*! accepts vector v*/
361
#define VEC_UNPROJECT(vp,v,n) \
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]; \
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) \
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]; \
383
Takes two vectors a, b, blends them together with two scalars */
384
#define VEC_BLEND_AB(vr,sa,a,sb,b) \
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]; \
392
Takes two vectors a, b, blends them together with s <=1 */
393
#define VEC_BLEND(vr,a,b,s) VEC_BLEND_AB(vr,(1-s),a,s,b)
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];
397
//! Finds the bigger cartesian coordinate from a vector
398
#define VEC_MAYOR_COORD(vec, maxc)\
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);\
404
//! Finds the 2 smallest cartesian coordinates from a vector
405
#define VEC_MINOR_AXES(vec, i0, i1)\
407
VEC_MAYOR_COORD(vec,i0);\
415
#define VEC_EQUAL(v1,v2) (v1[0]==v2[0]&&v1[1]==v2[1]&&v1[2]==v2[2])
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]))
421
#define X_AXIS_CROSS_VEC(dst,src)\
428
#define Y_AXIS_CROSS_VEC(dst,src)\
435
#define Z_AXIS_CROSS_VEC(dst,src)\
447
/// initialize matrix
448
#define IDENTIFY_MATRIX_3X3(m) \
463
/*! initialize matrix */
464
#define IDENTIFY_MATRIX_4X4(m) \
487
/*! initialize matrix */
488
#define ZERO_MATRIX_4X4(m) \
511
/*! matrix rotation X */
512
#define ROTX_CS(m,cosine,sine) \
514
/* rotation about the x-axis */ \
522
m[1][1] = (cosine); \
528
m[2][2] = (cosine); \
537
/*! matrix rotation Y */
538
#define ROTY_CS(m,cosine,sine) \
540
/* rotation about the y-axis */ \
542
m[0][0] = (cosine); \
554
m[2][2] = (cosine); \
563
/*! matrix rotation Z */
564
#define ROTZ_CS(m,cosine,sine) \
566
/* rotation about the z-axis */ \
568
m[0][0] = (cosine); \
574
m[1][1] = (cosine); \
590
#define COPY_MATRIX_2X2(b,a) \
602
#define COPY_MATRIX_2X3(b,a) \
615
#define COPY_MATRIX_3X3(b,a) \
632
#define COPY_MATRIX_4X4(b,a) \
656
/*! matrix transpose */
657
#define TRANSPOSE_MATRIX_2X2(b,a) \
667
/*! matrix transpose */
668
#define TRANSPOSE_MATRIX_3X3(b,a) \
684
/*! matrix transpose */
685
#define TRANSPOSE_MATRIX_4X4(b,a) \
709
/*! multiply matrix by scalar */
710
#define SCALE_MATRIX_2X2(b,s,a) \
712
b[0][0] = (s) * a[0][0]; \
713
b[0][1] = (s) * a[0][1]; \
715
b[1][0] = (s) * a[1][0]; \
716
b[1][1] = (s) * a[1][1]; \
720
/*! multiply matrix by scalar */
721
#define SCALE_MATRIX_3X3(b,s,a) \
723
b[0][0] = (s) * a[0][0]; \
724
b[0][1] = (s) * a[0][1]; \
725
b[0][2] = (s) * a[0][2]; \
727
b[1][0] = (s) * a[1][0]; \
728
b[1][1] = (s) * a[1][1]; \
729
b[1][2] = (s) * a[1][2]; \
731
b[2][0] = (s) * a[2][0]; \
732
b[2][1] = (s) * a[2][1]; \
733
b[2][2] = (s) * a[2][2]; \
737
/*! multiply matrix by scalar */
738
#define SCALE_MATRIX_4X4(b,s,a) \
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]; \
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]; \
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]; \
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]; \
762
/*! multiply matrix by scalar */
763
#define SCALE_VEC_MATRIX_2X2(b,svec,a) \
765
b[0][0] = svec[0] * a[0][0]; \
766
b[1][0] = svec[0] * a[1][0]; \
768
b[0][1] = svec[1] * a[0][1]; \
769
b[1][1] = svec[1] * a[1][1]; \
773
/*! multiply matrix by scalar. Each columns is scaled by each scalar vector component */
774
#define SCALE_VEC_MATRIX_3X3(b,svec,a) \
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]; \
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]; \
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]; \
790
/*! multiply matrix by scalar */
791
#define SCALE_VEC_MATRIX_4X4(b,svec,a) \
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]; \
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]; \
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]; \
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]; \
815
/*! multiply matrix by scalar */
816
#define ACCUM_SCALE_MATRIX_2X2(b,s,a) \
818
b[0][0] += (s) * a[0][0]; \
819
b[0][1] += (s) * a[0][1]; \
821
b[1][0] += (s) * a[1][0]; \
822
b[1][1] += (s) * a[1][1]; \
826
/*! multiply matrix by scalar */
827
#define ACCUM_SCALE_MATRIX_3X3(b,s,a) \
829
b[0][0] += (s) * a[0][0]; \
830
b[0][1] += (s) * a[0][1]; \
831
b[0][2] += (s) * a[0][2]; \
833
b[1][0] += (s) * a[1][0]; \
834
b[1][1] += (s) * a[1][1]; \
835
b[1][2] += (s) * a[1][2]; \
837
b[2][0] += (s) * a[2][0]; \
838
b[2][1] += (s) * a[2][1]; \
839
b[2][2] += (s) * a[2][2]; \
843
/*! multiply matrix by scalar */
844
#define ACCUM_SCALE_MATRIX_4X4(b,s,a) \
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]; \
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]; \
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]; \
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]; \
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) \
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]; \
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]; \
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) \
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]; \
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]; \
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]; \
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) \
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];\
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];\
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];\
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];\
923
/*! matrix times vector */
924
#define MAT_DOT_VEC_2X2(p,m,v) \
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]; \
931
/*! matrix times vector */
932
#define MAT_DOT_VEC_3X3(p,m,v) \
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]; \
940
/*! matrix times vector
943
#define MAT_DOT_VEC_4X4(p,m,v) \
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]; \
951
/*! matrix times vector
954
Last column is added as the position
956
#define MAT_DOT_VEC_3X4(p,m,v) \
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]; \
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) \
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]; \
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) \
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]; \
983
//! Transform a plane
984
#define MAT_TRANSFORM_PLANE_4X4(pout,m,plane)\
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];\
994
/** inverse transpose of matrix times vector
996
* This macro computes inverse transpose of matrix m,
997
* and multiplies vector v into it, to yeild vector p
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.
1003
#define INV_TRANSP_MAT_DOT_VEC_2X2(p,m,v) \
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]; \
1011
/* if matrix not singular, and not orthonormal, then renormalize */ \
1012
if ((det!=1.0f) && (det != 0.0f)) { \
1020
/** transform normal vector by inverse transpose of matrix
1021
* and then renormalize the vector
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.
1027
#define NORM_XFORM_2X2(p,m,v) \
1031
/* do nothing if off-diagonals are zero and diagonals are \
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]; \
1037
len = p[0]*p[0] + p[1]*p[1]; \
1038
GIM_INV_SQRT(len,len); \
1042
VEC_COPY_2 (p, v); \
1047
/** outer product of vector times vector transpose
1049
* The outer product of vector v and vector transpose t yeilds
1052
#define OUTER_PRODUCT_2X2(m,v,t) \
1054
m[0][0] = v[0] * t[0]; \
1055
m[0][1] = v[0] * t[1]; \
1057
m[1][0] = v[1] * t[0]; \
1058
m[1][1] = v[1] * t[1]; \
1062
/** outer product of vector times vector transpose
1064
* The outer product of vector v and vector transpose t yeilds
1067
#define OUTER_PRODUCT_3X3(m,v,t) \
1069
m[0][0] = v[0] * t[0]; \
1070
m[0][1] = v[0] * t[1]; \
1071
m[0][2] = v[0] * t[2]; \
1073
m[1][0] = v[1] * t[0]; \
1074
m[1][1] = v[1] * t[1]; \
1075
m[1][2] = v[1] * t[2]; \
1077
m[2][0] = v[2] * t[0]; \
1078
m[2][1] = v[2] * t[1]; \
1079
m[2][2] = v[2] * t[2]; \
1083
/** outer product of vector times vector transpose
1085
* The outer product of vector v and vector transpose t yeilds
1088
#define OUTER_PRODUCT_4X4(m,v,t) \
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]; \
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]; \
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]; \
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]; \
1112
/** outer product of vector times vector transpose
1114
* The outer product of vector v and vector transpose t yeilds
1117
#define ACCUM_OUTER_PRODUCT_2X2(m,v,t) \
1119
m[0][0] += v[0] * t[0]; \
1120
m[0][1] += v[0] * t[1]; \
1122
m[1][0] += v[1] * t[0]; \
1123
m[1][1] += v[1] * t[1]; \
1127
/** outer product of vector times vector transpose
1129
* The outer product of vector v and vector transpose t yeilds
1132
#define ACCUM_OUTER_PRODUCT_3X3(m,v,t) \
1134
m[0][0] += v[0] * t[0]; \
1135
m[0][1] += v[0] * t[1]; \
1136
m[0][2] += v[0] * t[2]; \
1138
m[1][0] += v[1] * t[0]; \
1139
m[1][1] += v[1] * t[1]; \
1140
m[1][2] += v[1] * t[2]; \
1142
m[2][0] += v[2] * t[0]; \
1143
m[2][1] += v[2] * t[1]; \
1144
m[2][2] += v[2] * t[2]; \
1148
/** outer product of vector times vector transpose
1150
* The outer product of vector v and vector transpose t yeilds
1153
#define ACCUM_OUTER_PRODUCT_4X4(m,v,t) \
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]; \
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]; \
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]; \
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]; \
1177
/** determinant of matrix
1179
* Computes determinant of matrix m, returning d
1181
#define DETERMINANT_2X2(d,m) \
1183
d = m[0][0] * m[1][1] - m[0][1] * m[1][0]; \
1187
/** determinant of matrix
1189
* Computes determinant of matrix m, returning d
1191
#define DETERMINANT_3X3(d,m) \
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]); \
1199
/** i,j,th cofactor of a 4x4 matrix
1202
#define COFACTOR_4X4_IJ(fac,m,i,j) \
1204
GUINT __ii[4], __jj[4], __k; \
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; \
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]]);\
1219
if ( __k != (__k/2)*2) { \
1225
/** determinant of matrix
1227
* Computes determinant of matrix m, returning d
1229
#define DETERMINANT_4X4(d,m) \
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; \
1243
/** cofactor of matrix
1245
* Computes cofactor of matrix m, returning a
1247
#define COFACTOR_2X2(a,m) \
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]; \
1256
/** cofactor of matrix
1258
* Computes cofactor of matrix m, returning a
1260
#define COFACTOR_3X3(a,m) \
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]); \
1274
/** cofactor of matrix
1276
* Computes cofactor of matrix m, returning a
1278
#define COFACTOR_4X4(a,m) \
1282
for (i=0; i<4; i++) { \
1283
for (j=0; j<4; j++) { \
1284
COFACTOR_4X4_IJ (a[i][j], m, i, j); \
1290
/** adjoint of matrix
1292
* Computes adjoint of matrix m, returning a
1293
* (Note that adjoint is just the transpose of the cofactor matrix)
1295
#define ADJOINT_2X2(a,m) \
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]; \
1304
/** adjoint of matrix
1306
* Computes adjoint of matrix m, returning a
1307
* (Note that adjoint is just the transpose of the cofactor matrix)
1309
#define ADJOINT_3X3(a,m) \
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]); \
1323
/** adjoint of matrix
1325
* Computes adjoint of matrix m, returning a
1326
* (Note that adjoint is just the transpose of the cofactor matrix)
1328
#define ADJOINT_4X4(a,m) \
1332
for (_i_=0; _i_<4; _i_++) { \
1333
for (_j_=0; _j_<4; _j_++) { \
1334
COFACTOR_4X4_IJ (a[_j_][_i_], m, _i_, _j_); \
1340
/** compute adjoint of matrix and scale
1342
* Computes adjoint of matrix m, scales it by s, returning a
1344
#define SCALE_ADJOINT_2X2(a,s,m) \
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]; \
1353
/** compute adjoint of matrix and scale
1355
* Computes adjoint of matrix m, scales it by s, returning a
1357
#define SCALE_ADJOINT_3X3(a,s,m) \
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]); \
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]); \
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]); \
1373
/** compute adjoint of matrix and scale
1375
* Computes adjoint of matrix m, scales it by s, returning a
1377
#define SCALE_ADJOINT_4X4(a,s,m) \
1380
for (_i_=0; _i_<4; _i_++) { \
1381
for (_j_=0; _j_<4; _j_++) { \
1382
COFACTOR_4X4_IJ (a[_j_][_i_], m, _i_, _j_); \
1388
/** inverse of matrix
1390
* Compute inverse of matrix a, returning determinant m and
1393
#define INVERT_2X2(b,det,a) \
1396
DETERMINANT_2X2 (det, a); \
1397
_tmp_ = 1.0 / (det); \
1398
SCALE_ADJOINT_2X2 (b, _tmp_, a); \
1402
/** inverse of matrix
1404
* Compute inverse of matrix a, returning determinant m and
1407
#define INVERT_3X3(b,det,a) \
1410
DETERMINANT_3X3 (det, a); \
1411
_tmp_ = 1.0 / (det); \
1412
SCALE_ADJOINT_3X3 (b, _tmp_, a); \
1416
/** inverse of matrix
1418
* Compute inverse of matrix a, returning determinant m and
1421
#define INVERT_4X4(b,det,a) \
1424
DETERMINANT_4X4 (det, a); \
1425
_tmp_ = 1.0 / (det); \
1426
SCALE_ADJOINT_4X4 (b, _tmp_, a); \
1429
//! Get the triple(3) row of a transform matrix
1430
#define MAT_GET_ROW(mat,vec3,rowindex)\
1432
vec3[0] = mat[rowindex][0];\
1433
vec3[1] = mat[rowindex][1];\
1434
vec3[2] = mat[rowindex][2]; \
1437
//! Get the tuple(2) row of a transform matrix
1438
#define MAT_GET_ROW2(mat,vec2,rowindex)\
1440
vec2[0] = mat[rowindex][0];\
1441
vec2[1] = mat[rowindex][1];\
1445
//! Get the quad (4) row of a transform matrix
1446
#define MAT_GET_ROW4(mat,vec4,rowindex)\
1448
vec4[0] = mat[rowindex][0];\
1449
vec4[1] = mat[rowindex][1];\
1450
vec4[2] = mat[rowindex][2];\
1451
vec4[3] = mat[rowindex][3];\
1454
//! Get the triple(3) col of a transform matrix
1455
#define MAT_GET_COL(mat,vec3,colindex)\
1457
vec3[0] = mat[0][colindex];\
1458
vec3[1] = mat[1][colindex];\
1459
vec3[2] = mat[2][colindex]; \
1462
//! Get the tuple(2) col of a transform matrix
1463
#define MAT_GET_COL2(mat,vec2,colindex)\
1465
vec2[0] = mat[0][colindex];\
1466
vec2[1] = mat[1][colindex];\
1470
//! Get the quad (4) col of a transform matrix
1471
#define MAT_GET_COL4(mat,vec4,colindex)\
1473
vec4[0] = mat[0][colindex];\
1474
vec4[1] = mat[1][colindex];\
1475
vec4[2] = mat[2][colindex];\
1476
vec4[3] = mat[3][colindex];\
1479
//! Get the triple(3) col of a transform matrix
1480
#define MAT_GET_X(mat,vec3)\
1482
MAT_GET_COL(mat,vec3,0);\
1485
//! Get the triple(3) col of a transform matrix
1486
#define MAT_GET_Y(mat,vec3)\
1488
MAT_GET_COL(mat,vec3,1);\
1491
//! Get the triple(3) col of a transform matrix
1492
#define MAT_GET_Z(mat,vec3)\
1494
MAT_GET_COL(mat,vec3,2);\
1498
//! Get the triple(3) col of a transform matrix
1499
#define MAT_SET_X(mat,vec3)\
1501
mat[0][0] = vec3[0];\
1502
mat[1][0] = vec3[1];\
1503
mat[2][0] = vec3[2];\
1506
//! Get the triple(3) col of a transform matrix
1507
#define MAT_SET_Y(mat,vec3)\
1509
mat[0][1] = vec3[0];\
1510
mat[1][1] = vec3[1];\
1511
mat[2][1] = vec3[2];\
1514
//! Get the triple(3) col of a transform matrix
1515
#define MAT_SET_Z(mat,vec3)\
1517
mat[0][2] = vec3[0];\
1518
mat[1][2] = vec3[1];\
1519
mat[2][2] = vec3[2];\
1523
//! Get the triple(3) col of a transform matrix
1524
#define MAT_GET_TRANSLATION(mat,vec3)\
1526
vec3[0] = mat[0][3];\
1527
vec3[1] = mat[1][3];\
1528
vec3[2] = mat[2][3]; \
1531
//! Set the triple(3) col of a transform matrix
1532
#define MAT_SET_TRANSLATION(mat,vec3)\
1534
mat[0][3] = vec3[0];\
1535
mat[1][3] = vec3[1];\
1536
mat[2][3] = vec3[2]; \
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])
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])
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])
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])
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])
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])
1560
/*!Transpose matrix times vector
1562
and m is a mat4f<br>
1564
#define INV_MAT_DOT_VEC_3X3(p,m,v) \
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); \
1573
#endif // GIM_VECTOR_H_INCLUDED
added
removed
tests/bullet/src/BulletCollision/Gimpact/gim_linear_math.h
