src/gallium/state_trackers/vega/matrix.h

   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
  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
  83 static INLINE void matrix_init(struct matrix *mat,
  84                                const VGfloat *val)
  85 {
  86    memcpy(mat->m, val, sizeof(VGfloat) * 9);
  87 }
  88
  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
  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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 297 static INLINE VGboolean matrix_is_invertible(struct matrix *mat)
 298 {
 299    return !floatIsZero(matrix_determinant(mat));
 300 }
 301
 302
 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
 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
 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
 395 static INLINE VGboolean null_line(const VGfloat *l)
 396 {
 397    return floatsEqual(l[0], l[2]) && floatsEqual(l[1], l[3]);
 398 }
 399
 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
 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
 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
 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
 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
 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
 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