1 /*
2 * Copyright (C) 1999-2005 Brian Paul All Rights Reserved.
3 *
4 * Permission is hereby granted, free of charge, to any person obtaining a
5 * copy of this software and associated documentation files (the "Software"),
6 * to deal in the Software without restriction, including without limitation
7 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
8 * and/or sell copies of the Software, and to permit persons to whom the
9 * Software is furnished to do so, subject to the following conditions:
10 *
11 * The above copyright notice and this permission notice shall be included
12 * in all copies or substantial portions of the Software.
13 *
14 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
15 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
17 * BRIAN PAUL BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN
18 * AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
19 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
20 *
21 * From Mesa 3-D graphics library.
22 */
23
24 #include <string.h>
25 #include <math.h>
26
27 #include <compiz-core.h>
28
29 /**
30 * Identity matrix.
31 */
32 static float identity[16] = {
33 1.0, 0.0, 0.0, 0.0,
34 0.0, 1.0, 0.0, 0.0,
35 0.0, 0.0, 1.0, 0.0,
36 0.0, 0.0, 0.0, 1.0
37 };
38
39 #define A(row, col) a[(col << 2) + row]
40 #define B(row, col) b[(col << 2) + row]
41 #define P(row, col) product[(col << 2) + row]
42
43 /**
44 * Perform a full 4x4 matrix multiplication.
45 *
46 * \param a matrix.
47 * \param b matrix.
48 * \param product will receive the product of \p a and \p b.
49 *
50 * \warning Is assumed that \p product != \p b. \p product == \p a is allowed.
51 *
52 * \note KW: 4*16 = 64 multiplications
53 *
54 * \author This \c matmul was contributed by Thomas Malik
55 */
56 static void
matmul4(float * product,const float * a,const float * b)57 matmul4 (float *product,
58 const float *a,
59 const float *b)
60 {
61 int i;
62
63 for (i = 0; i < 4; i++)
64 {
65 const float ai0 = A(i,0), ai1 = A(i,1), ai2 = A(i,2), ai3 = A(i,3);
66
67 P(i,0) = ai0 * B(0,0) + ai1 * B(1,0) + ai2 * B(2,0) + ai3 * B(3,0);
68 P(i,1) = ai0 * B(0,1) + ai1 * B(1,1) + ai2 * B(2,1) + ai3 * B(3,1);
69 P(i,2) = ai0 * B(0,2) + ai1 * B(1,2) + ai2 * B(2,2) + ai3 * B(3,2);
70 P(i,3) = ai0 * B(0,3) + ai1 * B(1,3) + ai2 * B(2,3) + ai3 * B(3,3);
71 }
72 }
73
74 void
matrixMultiply(CompTransform * product,const CompTransform * transformA,const CompTransform * transformB)75 matrixMultiply (CompTransform *product,
76 const CompTransform *transformA,
77 const CompTransform *transformB)
78 {
79 matmul4 (product->m, transformA->m, transformB->m);
80 }
81
82 /**
83 * Multiply the 1x4 vector v with the 4x4 matrix a.
84 *
85 * \param a matrix.
86 * \param v vector.
87 * \param product will receive the product of \p a and \p v.
88 *
89 */
90 void
matrixMultiplyVector(CompVector * product,const CompVector * vector,const CompTransform * transform)91 matrixMultiplyVector (CompVector *product,
92 const CompVector *vector,
93 const CompTransform *transform)
94 {
95 float vec[4];
96 const float *a = transform->m;
97 const float *b = vector->v;
98 int i;
99
100 for (i = 0; i < 4; i++)
101 {
102 vec[i] = A(i,0) * B(0,0) + A(i,1) * B(1,0) +
103 A(i,2) * B(2,0) + A(i,3) * B(3,0);
104 }
105
106 memcpy (product->v, vec, sizeof (vec));
107 }
108
109 void
matrixVectorDiv(CompVector * vector)110 matrixVectorDiv (CompVector *vector)
111 {
112 int i;
113
114 for (i = 0; i < 4; i++)
115 vector->v[i] /= vector->v[3];
116 }
117
118 #undef A
119 #undef B
120 #undef P
121
122 /**
123 * Generate a 4x4 transformation matrix from glRotate parameters, and
124 * post-multiply the input matrix by it.
125 *
126 * \author
127 * This function was contributed by Erich Boleyn (erich@uruk.org).
128 * Optimizations contributed by Rudolf Opalla (rudi@khm.de).
129 */
130 void
matrixRotate(CompTransform * transform,float angle,float x,float y,float z)131 matrixRotate (CompTransform *transform,
132 float angle,
133 float x,
134 float y,
135 float z)
136 {
137 float xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c, s, c;
138 float m[16];
139 Bool optimized;
140
141 s = (float) sin (angle * DEG2RAD);
142 c = (float) cos (angle * DEG2RAD);
143
144 memcpy (m, identity, sizeof (float) * 16);
145 optimized = FALSE;
146
147 #define M(row, col) m[col * 4 + row]
148
149 if (x == 0.0f)
150 {
151 if (y == 0.0f)
152 {
153 if (z != 0.0f)
154 {
155 optimized = TRUE;
156 /* rotate only around z-axis */
157 M(0,0) = c;
158 M(1,1) = c;
159 if (z < 0.0f)
160 {
161 M(0,1) = s;
162 M(1,0) = -s;
163 }
164 else
165 {
166 M(0,1) = -s;
167 M(1,0) = s;
168 }
169 }
170 }
171 else if (z == 0.0f)
172 {
173 optimized = TRUE;
174 /* rotate only around y-axis */
175 M(0,0) = c;
176 M(2,2) = c;
177 if (y < 0.0f)
178 {
179 M(0,2) = -s;
180 M(2,0) = s;
181 }
182 else
183 {
184 M(0,2) = s;
185 M(2,0) = -s;
186 }
187 }
188 }
189 else if (y == 0.0f)
190 {
191 if (z == 0.0f)
192 {
193 optimized = TRUE;
194 /* rotate only around x-axis */
195 M(1,1) = c;
196 M(2,2) = c;
197 if (x < 0.0f)
198 {
199 M(1,2) = s;
200 M(2,1) = -s;
201 }
202 else
203 {
204 M(1,2) = -s;
205 M(2,1) = s;
206 }
207 }
208 }
209
210 if (!optimized)
211 {
212 const float mag = sqrtf (x * x + y * y + z * z);
213
214 if (mag <= 1.0e-4)
215 {
216 /* no rotation, leave mat as-is */
217 return;
218 }
219
220 x /= mag;
221 y /= mag;
222 z /= mag;
223
224
225 /*
226 * Arbitrary axis rotation matrix.
227 *
228 * This is composed of 5 matrices, Rz, Ry, T, Ry', Rz', multiplied
229 * like so: Rz * Ry * T * Ry' * Rz'. T is the final rotation
230 * (which is about the X-axis), and the two composite transforms
231 * Ry' * Rz' and Rz * Ry are (respectively) the rotations necessary
232 * from the arbitrary axis to the X-axis then back. They are
233 * all elementary rotations.
234 *
235 * Rz' is a rotation about the Z-axis, to bring the axis vector
236 * into the x-z plane. Then Ry' is applied, rotating about the
237 * Y-axis to bring the axis vector parallel with the X-axis. The
238 * rotation about the X-axis is then performed. Ry and Rz are
239 * simply the respective inverse transforms to bring the arbitrary
240 * axis back to it's original orientation. The first transforms
241 * Rz' and Ry' are considered inverses, since the data from the
242 * arbitrary axis gives you info on how to get to it, not how
243 * to get away from it, and an inverse must be applied.
244 *
245 * The basic calculation used is to recognize that the arbitrary
246 * axis vector (x, y, z), since it is of unit length, actually
247 * represents the sines and cosines of the angles to rotate the
248 * X-axis to the same orientation, with theta being the angle about
249 * Z and phi the angle about Y (in the order described above)
250 * as follows:
251 *
252 * cos ( theta ) = x / sqrt ( 1 - z^2 )
253 * sin ( theta ) = y / sqrt ( 1 - z^2 )
254 *
255 * cos ( phi ) = sqrt ( 1 - z^2 )
256 * sin ( phi ) = z
257 *
258 * Note that cos ( phi ) can further be inserted to the above
259 * formulas:
260 *
261 * cos ( theta ) = x / cos ( phi )
262 * sin ( theta ) = y / sin ( phi )
263 *
264 * ...etc. Because of those relations and the standard trigonometric
265 * relations, it is pssible to reduce the transforms down to what
266 * is used below. It may be that any primary axis chosen will give the
267 * same results (modulo a sign convention) using thie method.
268 *
269 * Particularly nice is to notice that all divisions that might
270 * have caused trouble when parallel to certain planes or
271 * axis go away with care paid to reducing the expressions.
272 * After checking, it does perform correctly under all cases, since
273 * in all the cases of division where the denominator would have
274 * been zero, the numerator would have been zero as well, giving
275 * the expected result.
276 */
277
278 xx = x * x;
279 yy = y * y;
280 zz = z * z;
281 xy = x * y;
282 yz = y * z;
283 zx = z * x;
284 xs = x * s;
285 ys = y * s;
286 zs = z * s;
287 one_c = 1.0f - c;
288
289 /* We already hold the identity-matrix so we can skip some statements */
290 M(0,0) = (one_c * xx) + c;
291 M(0,1) = (one_c * xy) - zs;
292 M(0,2) = (one_c * zx) + ys;
293 /* M(0,3) = 0.0F; */
294
295 M(1,0) = (one_c * xy) + zs;
296 M(1,1) = (one_c * yy) + c;
297 M(1,2) = (one_c * yz) - xs;
298 /* M(1,3) = 0.0F; */
299
300 M(2,0) = (one_c * zx) - ys;
301 M(2,1) = (one_c * yz) + xs;
302 M(2,2) = (one_c * zz) + c;
303 /* M(2,3) = 0.0F; */
304
305 /*
306 M(3,0) = 0.0F;
307 M(3,1) = 0.0F;
308 M(3,2) = 0.0F;
309 M(3,3) = 1.0F;
310 */
311 }
312 #undef M
313
314 matmul4 (transform->m, transform->m, m);
315 }
316
317 /**
318 * Multiply a matrix with a general scaling matrix.
319 *
320 * \param matrix matrix.
321 * \param x x axis scale factor.
322 * \param y y axis scale factor.
323 * \param z z axis scale factor.
324 *
325 * Multiplies in-place the elements of \p matrix by the scale factors.
326 */
327 void
matrixScale(CompTransform * transform,float x,float y,float z)328 matrixScale (CompTransform *transform,
329 float x,
330 float y,
331 float z)
332 {
333 float *m = transform->m;
334
335 m[0] *= x; m[4] *= y; m[8] *= z;
336 m[1] *= x; m[5] *= y; m[9] *= z;
337 m[2] *= x; m[6] *= y; m[10] *= z;
338 m[3] *= x; m[7] *= y; m[11] *= z;
339 }
340
341 /**
342 * Multiply a matrix with a translation matrix.
343 *
344 * \param matrix matrix.
345 * \param x translation vector x coordinate.
346 * \param y translation vector y coordinate.
347 * \param z translation vector z coordinate.
348 *
349 * Adds the translation coordinates to the elements of \p matrix in-place.
350 */
351 void
matrixTranslate(CompTransform * transform,float x,float y,float z)352 matrixTranslate (CompTransform *transform,
353 float x,
354 float y,
355 float z)
356 {
357 float *m = transform->m;
358
359 m[12] = m[0] * x + m[4] * y + m[8] * z + m[12];
360 m[13] = m[1] * x + m[5] * y + m[9] * z + m[13];
361 m[14] = m[2] * x + m[6] * y + m[10] * z + m[14];
362 m[15] = m[3] * x + m[7] * y + m[11] * z + m[15];
363 }
364
365 void
matrixGetIdentity(CompTransform * transform)366 matrixGetIdentity (CompTransform *transform)
367 {
368 memcpy (transform->m, identity, sizeof (float) * 16);
369 }
370