KrisLibrary  1.0.0
matinline.h
1 #ifndef MATRIX_INLINES_H
2 #define MATRIX_INLINES_H
3 
4 #include "vecinline.h"
5 
6 namespace Math3D {
7 
8 /*****************************matrix operations*******************************/
9 
10 //x = 0
11 inline void matrix4_zero(matrix4_t x)
12 {
13  int i,j;
14  for(i=0; i<4; i++)
15  for(j=0; j<4; j++)
16  x[i][j] = Zero;
17 }
18 
19 //x = I
20 inline void matrix4_identity(matrix4_t x)
21 {
22  matrix4_zero(x);
23  x[0][0] = x[1][1] = x[2][2] = x[3][3] = One;
24 }
25 
26 //x = ( xb yb zb trans)
27 inline void matrix4_from_basis(matrix4_t x, const vec4_t xbasis, const vec4_t ybasis, const vec4_t zbasis, const vec4_t trans)
28 {
29  vec4_equal(x[0], xbasis);
30  vec4_equal(x[1], ybasis);
31  vec4_equal(x[2], zbasis);
32  vec4_equal(x[3], trans);
33 }
34 
35 //reverse of above
36 inline void matrix4_to_basis(const matrix4_t x, vec4_t xbasis, vec4_t ybasis, vec4_t zbasis, vec4_t trans)
37 {
38  vec4_equal(xbasis, x[0]);
39  vec4_equal(ybasis, x[1]);
40  vec4_equal(zbasis, x[2]);
41  vec4_equal(trans, x[3]);
42 }
43 
44 //x = a
45 inline void matrix4_equal(matrix4_t x, const matrix4_t a)
46 {
47  int i,j;
48  for(i=0; i<4; i++)
49  for(j=0; j<4; j++)
50  x[i][j] = a[i][j];
51 }
52 
53 //x = a+b
54 inline void matrix4_add(matrix4_t x, const matrix4_t a, const matrix4_t b)
55 {
56  int i,j;
57  for(i=0; i<4; i++)
58  for(j=0; j<4; j++)
59  x[i][j] = a[i][j] + b[i][j];
60 }
61 
62 //x = a-b
63 inline void matrix4_sub(matrix4_t x, const matrix4_t a, const matrix4_t b)
64 {
65  int i,j;
66  for(i=0; i<4; i++)
67  for(j=0; j<4; j++)
68  x[i][j] = a[i][j] - b[i][j];
69 }
70 
71 //x = a*b
72 inline void matrix4_multiply(matrix4_t x, const matrix4_t a, const matrix4_t b)
73 {
74  int i,j,k;
75 
76  matrix4_t temp;
77  matrix4_zero(temp);
78 
79  for(i=0; i<4; i++ )
80  for(j=0; j<4; j++ )
81  for(k=0; k<4; k++ )
82  temp[i][j] += b[i][k] * a[k][j];
83 
84  matrix4_equal(x, temp);
85 }
86 
87 //x = a^t
88 inline void matrix4_transpose(matrix4_t x, const matrix4_t a)
89 {
90  int i,j;
91  for(i=0; i<4; i++)
92  for(j=0; j<4; j++)
93  x[i][j] = a[j][i];
94 }
95 
96 //x = A*v
97 inline void matrix4_vec3_multiply(vec3_t x, const matrix4_t a, const vec3_t v)
98 {
99  x[0] = a[0][0]*v[0] + a[1][0]*v[1] + a[2][0]*v[2] + a[3][0];
100  x[1] = a[0][1]*v[0] + a[1][1]*v[1] + a[2][1]*v[2] + a[3][1];
101  x[2] = a[0][2]*v[0] + a[1][2]*v[1] + a[2][2]*v[2] + a[3][2];
102 }
103 
104 //x = A*v (the 3*3 part)
105 inline void matrix4_normal_multiply(vec3_t x, const matrix4_t a, const vec3_t v)
106 {
107  x[0] = a[0][0]*v[0] + a[1][0]*v[1] + a[2][0]*v[2];
108  x[1] = a[0][1]*v[0] + a[1][1]*v[1] + a[2][1]*v[2];
109  x[2] = a[0][2]*v[0] + a[1][2]*v[1] + a[2][2]*v[2];
110 }
111 
112 //x = At*v
113 inline void matrix4_vec3_multiply_transpose(vec3_t x, const matrix4_t a, const vec3_t v)
114 {
115  x[0] = a[0][0]*v[0] + a[0][1]*v[1] + a[0][2]*v[2];
116  x[1] = a[1][0]*v[0] + a[1][1]*v[1] + a[1][2]*v[2];
117  x[2] = a[2][0]*v[0] + a[2][1]*v[1] + a[2][2]*v[2];
118 }
119 
120 #define matrix4_normal_multiply_transpose matrix4_vec3_multiply_transpose
121 
122 //x = A*v
123 inline void matrix4_vec4_multiply(vec4_t x, const matrix4_t a, const vec4_t v)
124 {
125  x[0] = a[0][0]*v[0] + a[1][0]*v[1] + a[2][0]*v[2] + a[3][0]*v[3];
126  x[1] = a[0][1]*v[0] + a[1][1]*v[1] + a[2][1]*v[2] + a[3][1]*v[3];
127  x[2] = a[0][2]*v[0] + a[1][2]*v[1] + a[2][2]*v[2] + a[3][2]*v[3];
128  x[2] = a[0][3]*v[0] + a[1][3]*v[1] + a[2][3]*v[2] + a[3][3]*v[3];
129 }
130 
131 
132 //scales the rotation part of the matrix by (s,s,s)
133 inline void matrix4_scale3(matrix4_t x, Real scale)
134 {
135  int i,j;
136 
137  for(i=0; i<3; i++)
138  for(j=0; j<3; j++)
139  x[i][j] *= scale;
140 }
141 
142 //scales the rotation part of the matrix by a 3 vector
143 inline void matrix4_scale3(matrix4_t x, const vec3_t v_scale)
144 {
145  int i,j;
146 
147  for(i=0; i<3; i++)
148  for(j=0; j<3; j++)
149  x[i][j] *= v_scale[i];
150 }
151 
152 inline Real matrix3_determinant(const matrix4_t a)
153 {
154  return a[0][0] * (a[1][1]*a[2][2] - a[2][1]*a[1][2])
155  + a[1][0] * (a[2][1]*a[0][2] - a[2][2]*a[0][1])
156  + a[2][0] * (a[0][1]*a[1][2] - a[0][2]*a[1][1]);
157 }
158 
159 //x = matrix such that x*w = (v x w) for all w
160 inline void matrix4_cross_product(matrix4_t x, const vec3_t v)
161 {
162  matrix4_zero(x);
163  x[1][0] = -v[2];
164  x[2][1] = -v[0];
165  x[2][0] = v[1];
166 
167  x[0][1] = v[2];
168  x[1][2] = v[0];
169  x[0][2] = -v[1];
170  x[3][3] = 1.0;
171 }
172 
173 inline void matrix4_uncross(vec3_t x, const matrix4_t a)
174 {
175  x[0] = a[1][2];
176  x[1] = a[2][0];
177  x[2] = a[0][1];
178 }
179 
180 
181 void matrix4_invert( matrix4_t x, const matrix4_t a );
182 
183 
184 void matrix4_rotation(matrix4_t x, const vec3_t r);
185 void matrix4_rotation_axis(vec3_t r, const matrix4_t R);
186 
187 inline void matrix4_rotate_x(matrix4_t x, Real fRads)
188 {
189  matrix4_identity(x);
190  Real cx = Cos(fRads);
191  Real sx = Sin(fRads);
192  x[1][1] = cx;
193  x[2][2] = cx;
194  x[1][2] = sx;
195  x[2][1] = -sx;
196 }
197 
198 inline void matrix4_rotate_y(matrix4_t x, Real fRads)
199 {
200  matrix4_identity(x);
201  Real cy = Cos(fRads);
202  Real sy = Sin(fRads);
203  x[0][0] = cy;
204  x[2][2] = cy;
205  x[0][2] = -sy;
206  x[2][0] = sy;
207 }
208 
209 inline void matrix4_rotate_z(matrix4_t x, Real fRads)
210 {
211  matrix4_identity(x);
212  Real cz = Cos(fRads);
213  Real sz = Sin(fRads);
214  x[0][0] = cz;
215  x[1][1] = cz;
216  x[0][1] = sz;
217  x[1][0] = -sz;
218 }
219 
220 
221 //x = matrix such that x*w = proj (w on v) for all w
222 //= v*v^t / <v,v>
223 inline void matrix4_projection(matrix4_t x, const vec3_t v)
224 {
225  vec3_t temp;
226  matrix4_identity(x);
227  vec3_make(temp, One,Zero,Zero);
228  vec3_project(x[0], temp, v);
229  vec3_make(temp, Zero,One,Zero);
230  vec3_project(x[1], temp, v);
231  vec3_make(temp, Zero,Zero,One);
232  vec3_project(x[2], temp, v);
233 }
234 
235 //x = matrix such that x*w = proj (w on plane defined by p) for all w
236 //= I - v*p^t / v^t*p
237 inline void matrix4_plane_projection(matrix4_t x, const vec4_t p, vec3_t v)
238 {
239  Real c = - One / vec3_dot(v,p);
240  for(int i=0; i<4; i++)
241  for(int j=0; j<4; j++)
242  {
243  if(i == 3)
244  x[i][j] = Zero;
245  else
246  x[i][j] = v[i]*p[j] * c;
247 
248  if(i == j)
249  x[i][j] += One;
250  }
251 }
252 
253 //if a is a rigid body transformation (a rotation + a translation)
254 //x = a^-1
255 //( R(r)*T(t) )^-1 = T(-t)*R(r)^t = R(r)^t * T(R(r)^t(-t))
256 inline void matrix4_rigid_body_inverse(matrix4_t x, const matrix4_t a)
257 {
258  matrix4_transpose(x, a);
259  x[0][3] = x[1][3] = x[2][3] = Zero;
260  vec3_t r;
261  matrix4_normal_multiply(r, x, a[3]);
262  vec3_negate(x[3], r);
263 }
264 
265 } //namespace Math
266 
267 #endif
Contains all the definitions in the Math3D package.
Definition: AnyGeometry.h:12