1 /**************************************************************************
2 *
3 * Copyright 2009 VMware, Inc. All Rights Reserved.
4 *
5 * Permission is hereby granted, free of charge, to any person obtaining a
6 * copy of this software and associated documentation files (the
7 * "Software"), to deal in the Software without restriction, including
8 * without limitation the rights to use, copy, modify, merge, publish,
9 * distribute, sub license, and/or sell copies of the Software, and to
10 * permit persons to whom the Software is furnished to do so, subject to
11 * the following conditions:
12 *
13 * The above copyright notice and this permission notice (including the
14 * next paragraph) shall be included in all copies or substantial portions
15 * of the Software.
16 *
17 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
18 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
19 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
20 * IN NO EVENT SHALL VMWARE AND/OR ITS SUPPLIERS BE LIABLE FOR
21 * ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
22 * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
23 * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
24 *
25 **************************************************************************/
26
27 #ifndef MATRIX_H
28 #define MATRIX_H
29
30 #include "VG/openvg.h"
31
32 #include "pipe/p_compiler.h"
33 #include "util/u_math.h"
34
35 #include <stdio.h>
36 #include <math.h>
37
38 #define floatsEqual(x, y) (fabs(x - y) <= 0.00001f * MIN2(fabs(x), fabs(y)))
39 #define floatIsZero(x) (floatsEqual((x) + 1, 1))
40 #define ABS(x) (fabsf(x))
41
42 #define DEGREES_TO_RADIANS(d) (0.0174532925199 * (d))
43 #define FLT_TO_INT(flt) float_to_int_floor(((VGuint*)&(flt))[0])
44
float_to_int_floor(VGuint bits)45 static INLINE VGint float_to_int_floor(VGuint bits)
46 {
47 int sign = (bits >> 31) ? -1 : 1;
48 int exp = ((bits >> 23) & 255) - 127;
49 int mant = bits & 0x007fffff;
50 int sh = 23 - exp;
51
52 /* abs(value) >= 2^31 -> clamp. */
53
54 if (exp >= 31)
55 return (VGint)((sign < 0) ? 0x80000000u : 0x7fffffffu);
56
57 /* abs(value) < 1 -> return -1 or 0. */
58
59 if (exp < 0)
60 return (sign < 0 && (exp > -127 || mant != 0)) ? -1 : 0;
61
62 /* abs(value) >= 2^23 -> shift left. */
63
64 mant |= 0x00800000;
65 if (sh <= 0)
66 return sign * (mant << -sh);
67
68 /* Negative -> add a rounding term. */
69
70 if (sign < 0)
71 mant += (1 << sh) - 1;
72
73 /* Shift right to obtain the result. */
74
75 return sign * (mant >> sh);
76 }
77
78
79 struct matrix {
80 VGfloat m[9];
81 };
82
matrix_init(struct matrix * mat,const VGfloat * val)83 static INLINE void matrix_init(struct matrix *mat,
84 const VGfloat *val)
85 {
86 memcpy(mat->m, val, sizeof(VGfloat) * 9);
87 }
88
matrix_inits(struct matrix * mat,VGfloat m11,VGfloat m12,VGfloat m13,VGfloat m21,VGfloat m22,VGfloat m23,VGfloat m31,VGfloat m32,VGfloat m33)89 static INLINE void matrix_inits(struct matrix *mat,
90 VGfloat m11, VGfloat m12, VGfloat m13,
91 VGfloat m21, VGfloat m22, VGfloat m23,
92 VGfloat m31, VGfloat m32, VGfloat m33)
93 {
94 mat->m[0] = m11; mat->m[1] = m12; mat->m[2] = m13;
95 mat->m[3] = m21; mat->m[4] = m22; mat->m[5] = m23;
96 mat->m[6] = m31; mat->m[7] = m32; mat->m[8] = m33;
97 }
98
matrix_load_identity(struct matrix * matrix)99 static INLINE void matrix_load_identity(struct matrix *matrix)
100 {
101 static const VGfloat identity[9] = {1.f, 0.f, 0.f,
102 0.f, 1.f, 0.f,
103 0.f, 0.f, 1.f};
104 memcpy(matrix->m, identity, sizeof(identity));
105 }
106
matrix_is_identity(struct matrix * matrix)107 static INLINE VGboolean matrix_is_identity(struct matrix *matrix)
108 {
109 return floatsEqual(matrix->m[0], 1) && floatIsZero(matrix->m[1]) &&
110 floatIsZero(matrix->m[2]) &&
111 floatIsZero(matrix->m[3]) && floatsEqual(matrix->m[4], 1) &&
112 floatIsZero(matrix->m[5]) &&
113 floatIsZero(matrix->m[6]) && floatIsZero(matrix->m[7]) &&
114 floatIsZero(matrix->m[8]);
115 }
116
matrix_is_affine(struct matrix * matrix)117 static INLINE VGboolean matrix_is_affine(struct matrix *matrix)
118 {
119 return floatIsZero(matrix->m[2]) && floatIsZero(matrix->m[5])
120 && floatsEqual(matrix->m[8], 1);
121 }
122
123
matrix_make_affine(struct matrix * matrix)124 static INLINE void matrix_make_affine(struct matrix *matrix)
125 {
126 matrix->m[2] = 0.f;
127 matrix->m[5] = 0.f;
128 matrix->m[8] = 1.f;
129 }
130
matrix_mult(struct matrix * dst,const struct matrix * src)131 static INLINE void matrix_mult(struct matrix *dst,
132 const struct matrix *src)
133 {
134 VGfloat m11 = dst->m[0]*src->m[0] + dst->m[3]*src->m[1] + dst->m[6]*src->m[2];
135 VGfloat m12 = dst->m[0]*src->m[3] + dst->m[3]*src->m[4] + dst->m[6]*src->m[5];
136 VGfloat m13 = dst->m[0]*src->m[6] + dst->m[3]*src->m[7] + dst->m[6]*src->m[8];
137
138 VGfloat m21 = dst->m[1]*src->m[0] + dst->m[4]*src->m[1] + dst->m[7]*src->m[2];
139 VGfloat m22 = dst->m[1]*src->m[3] + dst->m[4]*src->m[4] + dst->m[7]*src->m[5];
140 VGfloat m23 = dst->m[1]*src->m[6] + dst->m[4]*src->m[7] + dst->m[7]*src->m[8];
141
142 VGfloat m31 = dst->m[2]*src->m[0] + dst->m[5]*src->m[1] + dst->m[8]*src->m[2];
143 VGfloat m32 = dst->m[2]*src->m[3] + dst->m[5]*src->m[4] + dst->m[8]*src->m[5];
144 VGfloat m33 = dst->m[2]*src->m[6] + dst->m[5]*src->m[7] + dst->m[8]*src->m[8];
145
146 dst->m[0] = m11; dst->m[1] = m21; dst->m[2] = m31;
147 dst->m[3] = m12; dst->m[4] = m22; dst->m[5] = m32;
148 dst->m[6] = m13; dst->m[7] = m23; dst->m[8] = m33;
149 }
150
151
matrix_map_point(struct matrix * mat,VGfloat x,VGfloat y,VGfloat * out_x,VGfloat * out_y)152 static INLINE void matrix_map_point(struct matrix *mat,
153 VGfloat x, VGfloat y,
154 VGfloat *out_x, VGfloat *out_y)
155 {
156 /* to be able to do matrix_map_point(m, x, y, &x, &y) use
157 * temporaries */
158 VGfloat tmp_x = x, tmp_y = y;
159
160 *out_x = mat->m[0]*tmp_x + mat->m[3]*tmp_y + mat->m[6];
161 *out_y = mat->m[1]*tmp_x + mat->m[4]*tmp_y + mat->m[7];
162 if (!matrix_is_affine(mat)) {
163 VGfloat w = 1/(mat->m[2]*tmp_x + mat->m[5]*tmp_y + mat->m[8]);
164 *out_x *= w;
165 *out_y *= w;
166 }
167 }
168
matrix_translate(struct matrix * dst,VGfloat tx,VGfloat ty)169 static INLINE void matrix_translate(struct matrix *dst,
170 VGfloat tx, VGfloat ty)
171 {
172 if (!matrix_is_affine(dst)) {
173 struct matrix trans_matrix;
174 matrix_load_identity(&trans_matrix);
175 trans_matrix.m[6] = tx;
176 trans_matrix.m[7] = ty;
177 matrix_mult(dst, &trans_matrix);
178 } else {
179 dst->m[6] += tx*dst->m[0] + ty*dst->m[3];
180 dst->m[7] += ty*dst->m[4] + tx*dst->m[1];
181 }
182 }
183
matrix_scale(struct matrix * dst,VGfloat sx,VGfloat sy)184 static INLINE void matrix_scale(struct matrix *dst,
185 VGfloat sx, VGfloat sy)
186 {
187 if (!matrix_is_affine(dst)) {
188 struct matrix scale_matrix;
189 matrix_load_identity(&scale_matrix);
190 scale_matrix.m[0] = sx;
191 scale_matrix.m[4] = sy;
192 matrix_mult(dst, &scale_matrix);
193 } else {
194 dst->m[0] *= sx; dst->m[1] *= sx;
195 dst->m[3] *= sy; dst->m[4] *= sy;
196 }
197 }
198
matrix_shear(struct matrix * dst,VGfloat shx,VGfloat shy)199 static INLINE void matrix_shear(struct matrix *dst,
200 VGfloat shx, VGfloat shy)
201 {
202 struct matrix shear_matrix;
203 matrix_load_identity(&shear_matrix);
204 shear_matrix.m[1] = shy;
205 shear_matrix.m[3] = shx;
206 matrix_mult(dst, &shear_matrix);
207 }
208
matrix_rotate(struct matrix * dst,VGfloat angle)209 static INLINE void matrix_rotate(struct matrix *dst,
210 VGfloat angle)
211 {
212 struct matrix mat;
213 float sin_val = 0;
214 float cos_val = 0;
215
216
217 if (floatsEqual(angle, 90) || floatsEqual(angle, -270))
218 sin_val = 1.f;
219 else if (floatsEqual(angle, 270) || floatsEqual(angle, -90))
220 sin_val = -1.f;
221 else if (floatsEqual(angle, 180))
222 cos_val = -1.f;
223 else {
224 float radians = DEGREES_TO_RADIANS(angle);
225 sin_val = sin(radians);
226 cos_val = cos(radians);
227 }
228
229 if (!matrix_is_affine(dst)) {
230 matrix_load_identity(&mat);
231 mat.m[0] = cos_val; mat.m[1] = sin_val;
232 mat.m[3] = -sin_val; mat.m[4] = cos_val;
233
234 matrix_mult(dst, &mat);
235 } else {
236 VGfloat m11 = cos_val*dst->m[0] + sin_val*dst->m[3];
237 VGfloat m12 = cos_val*dst->m[1] + sin_val*dst->m[4];
238 VGfloat m21 = -sin_val*dst->m[0] + cos_val*dst->m[3];
239 VGfloat m22 = -sin_val*dst->m[1] + cos_val*dst->m[4];
240 dst->m[0] = m11; dst->m[1] = m12;
241 dst->m[3] = m21; dst->m[4] = m22;
242 }
243 }
244
245
matrix_determinant(struct matrix * mat)246 static INLINE VGfloat matrix_determinant(struct matrix *mat)
247 {
248 return mat->m[0]*(mat->m[8]*mat->m[4]-mat->m[7]*mat->m[5]) -
249 mat->m[3]*(mat->m[8]*mat->m[1]-mat->m[7]*mat->m[2])+
250 mat->m[6]*(mat->m[5]*mat->m[1]-mat->m[4]*mat->m[2]);
251 }
252
253
matrix_adjoint(struct matrix * mat)254 static INLINE void matrix_adjoint(struct matrix *mat)
255 {
256 VGfloat h[9];
257 h[0] = mat->m[4]*mat->m[8] - mat->m[5]*mat->m[7];
258 h[3] = mat->m[5]*mat->m[6] - mat->m[3]*mat->m[8];
259 h[6] = mat->m[3]*mat->m[7] - mat->m[4]*mat->m[6];
260 h[1] = mat->m[2]*mat->m[7] - mat->m[1]*mat->m[8];
261 h[4] = mat->m[0]*mat->m[8] - mat->m[2]*mat->m[6];
262 h[7] = mat->m[1]*mat->m[6] - mat->m[0]*mat->m[7];
263 h[2] = mat->m[1]*mat->m[5] - mat->m[2]*mat->m[4];
264 h[5] = mat->m[2]*mat->m[3] - mat->m[0]*mat->m[5];
265 h[8] = mat->m[0]*mat->m[4] - mat->m[1]*mat->m[3];
266
267
268 memcpy(mat->m, h, sizeof(VGfloat) * 9);
269 }
270
matrix_divs(struct matrix * mat,VGfloat s)271 static INLINE void matrix_divs(struct matrix *mat,
272 VGfloat s)
273 {
274 mat->m[0] /= s;
275 mat->m[1] /= s;
276 mat->m[2] /= s;
277 mat->m[3] /= s;
278 mat->m[4] /= s;
279 mat->m[5] /= s;
280 mat->m[6] /= s;
281 mat->m[7] /= s;
282 mat->m[8] /= s;
283 }
284
matrix_invert(struct matrix * mat)285 static INLINE VGboolean matrix_invert(struct matrix *mat)
286 {
287 VGfloat det = matrix_determinant(mat);
288
289 if (floatIsZero(det))
290 return VG_FALSE;
291
292 matrix_adjoint(mat);
293 matrix_divs(mat, det);
294 return VG_TRUE;
295 }
296
matrix_is_invertible(struct matrix * mat)297 static INLINE VGboolean matrix_is_invertible(struct matrix *mat)
298 {
299 return !floatIsZero(matrix_determinant(mat));
300 }
301
302
matrix_square_to_quad(VGfloat dx0,VGfloat dy0,VGfloat dx1,VGfloat dy1,VGfloat dx3,VGfloat dy3,VGfloat dx2,VGfloat dy2,struct matrix * mat)303 static INLINE VGboolean matrix_square_to_quad(VGfloat dx0, VGfloat dy0,
304 VGfloat dx1, VGfloat dy1,
305 VGfloat dx3, VGfloat dy3,
306 VGfloat dx2, VGfloat dy2,
307 struct matrix *mat)
308 {
309 VGfloat ax = dx0 - dx1 + dx2 - dx3;
310 VGfloat ay = dy0 - dy1 + dy2 - dy3;
311
312 if (floatIsZero(ax) && floatIsZero(ay)) {
313 /* affine case */
314 matrix_inits(mat,
315 dx1 - dx0, dy1 - dy0, 0,
316 dx2 - dx1, dy2 - dy1, 0,
317 dx0, dy0, 1);
318 } else {
319 VGfloat a, b, c, d, e, f, g, h;
320 VGfloat ax1 = dx1 - dx2;
321 VGfloat ax2 = dx3 - dx2;
322 VGfloat ay1 = dy1 - dy2;
323 VGfloat ay2 = dy3 - dy2;
324
325 /* determinants */
326 VGfloat gtop = ax * ay2 - ax2 * ay;
327 VGfloat htop = ax1 * ay - ax * ay1;
328 VGfloat bottom = ax1 * ay2 - ax2 * ay1;
329
330 if (!bottom)
331 return VG_FALSE;
332
333 g = gtop / bottom;
334 h = htop / bottom;
335
336 a = dx1 - dx0 + g * dx1;
337 b = dx3 - dx0 + h * dx3;
338 c = dx0;
339 d = dy1 - dy0 + g * dy1;
340 e = dy3 - dy0 + h * dy3;
341 f = dy0;
342
343 matrix_inits(mat,
344 a, d, g,
345 b, e, h,
346 c, f, 1.f);
347 }
348
349 return VG_TRUE;
350 }
351
matrix_quad_to_square(VGfloat sx0,VGfloat sy0,VGfloat sx1,VGfloat sy1,VGfloat sx2,VGfloat sy2,VGfloat sx3,VGfloat sy3,struct matrix * mat)352 static INLINE VGboolean matrix_quad_to_square(VGfloat sx0, VGfloat sy0,
353 VGfloat sx1, VGfloat sy1,
354 VGfloat sx2, VGfloat sy2,
355 VGfloat sx3, VGfloat sy3,
356 struct matrix *mat)
357 {
358 if (!matrix_square_to_quad(sx0, sy0, sx1, sy1,
359 sx2, sy2, sx3, sy3,
360 mat))
361 return VG_FALSE;
362
363 return matrix_invert(mat);
364 }
365
366
matrix_quad_to_quad(VGfloat dx0,VGfloat dy0,VGfloat dx1,VGfloat dy1,VGfloat dx2,VGfloat dy2,VGfloat dx3,VGfloat dy3,VGfloat sx0,VGfloat sy0,VGfloat sx1,VGfloat sy1,VGfloat sx2,VGfloat sy2,VGfloat sx3,VGfloat sy3,struct matrix * mat)367 static INLINE VGboolean matrix_quad_to_quad(VGfloat dx0, VGfloat dy0,
368 VGfloat dx1, VGfloat dy1,
369 VGfloat dx2, VGfloat dy2,
370 VGfloat dx3, VGfloat dy3,
371 VGfloat sx0, VGfloat sy0,
372 VGfloat sx1, VGfloat sy1,
373 VGfloat sx2, VGfloat sy2,
374 VGfloat sx3, VGfloat sy3,
375 struct matrix *mat)
376 {
377 struct matrix sqr_to_qd;
378
379 if (!matrix_square_to_quad(dx0, dy0, dx1, dy1,
380 dx2, dy2, dx3, dy3,
381 mat))
382 return VG_FALSE;
383
384 if (!matrix_quad_to_square(sx0, sy0, sx1, sy1,
385 sx2, sy2, sx3, sy3,
386 &sqr_to_qd))
387 return VG_FALSE;
388
389 matrix_mult(mat, &sqr_to_qd);
390
391 return VG_TRUE;
392 }
393
394
null_line(const VGfloat * l)395 static INLINE VGboolean null_line(const VGfloat *l)
396 {
397 return floatsEqual(l[0], l[2]) && floatsEqual(l[1], l[3]);
398 }
399
line_normal(float * l,float * norm)400 static INLINE void line_normal(float *l, float *norm)
401 {
402 norm[0] = l[0];
403 norm[1] = l[1];
404
405 norm[2] = l[0] + (l[3] - l[1]);
406 norm[3] = l[1] - (l[2] - l[0]);
407 }
408
line_normalize(float * l)409 static INLINE void line_normalize(float *l)
410 {
411 float x = l[2] - l[0];
412 float y = l[3] - l[1];
413 float len = sqrt(x*x + y*y);
414 l[2] = l[0] + x/len;
415 l[3] = l[1] + y/len;
416 }
417
line_length(VGfloat x1,VGfloat y1,VGfloat x2,VGfloat y2)418 static INLINE VGfloat line_length(VGfloat x1, VGfloat y1,
419 VGfloat x2, VGfloat y2)
420 {
421 VGfloat x = x2 - x1;
422 VGfloat y = y2 - y1;
423 return sqrt(x*x + y*y);
424 }
425
line_lengthv(const VGfloat * l)426 static INLINE VGfloat line_lengthv(const VGfloat *l)
427 {
428 VGfloat x = l[2] - l[0];
429 VGfloat y = l[3] - l[1];
430 return sqrt(x*x + y*y);
431 }
432
433
line_point_at(float * l,float t,float * pt)434 static INLINE void line_point_at(float *l, float t, float *pt)
435 {
436 float dx = l[2] - l[0];
437 float dy = l[3] - l[1];
438
439 pt[0] = l[0] + dx * t;
440 pt[1] = l[1] + dy * t;
441 }
442
vector_unit(float * vec)443 static INLINE void vector_unit(float *vec)
444 {
445 float len = sqrt(vec[0] * vec[0] + vec[1] * vec[1]);
446 vec[0] /= len;
447 vec[1] /= len;
448 }
449
line_normal_vector(float * line,float * vec)450 static INLINE void line_normal_vector(float *line, float *vec)
451 {
452 VGfloat normal[4];
453
454 line_normal(line, normal);
455
456 vec[0] = normal[2] - normal[0];
457 vec[1] = normal[3] - normal[1];
458
459 vector_unit(vec);
460 }
461
462 #endif
463