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