matrix.h revision b06de80843e7d096bed4ae03ddc5e2842f1876af 
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
45static 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
79struct matrix {
80   VGfloat m[9];
81};
82
83static INLINE void matrix_init(struct matrix *mat,
84                               const VGfloat *val)
85{
86   memcpy(mat->m, val, sizeof(VGfloat) * 9);
87}
88
89static 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
99static 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
107static 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
117static 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
124static 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
131static 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
152static 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
169static 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
184static 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
199static 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
209static 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
246static 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
254static 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
271static 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
285static 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
297static INLINE VGboolean matrix_is_invertible(struct matrix *mat)
298{
299   return !floatIsZero(matrix_determinant(mat));
300}
301
302
303static 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
352static 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
367static 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
395static INLINE VGboolean null_line(const VGfloat *l)
396{
397   return floatsEqual(l[0], l[2]) && floatsEqual(l[1], l[3]);
398}
399
400static 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
409static 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
418static 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
426static 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
434static 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
443static 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
450static 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