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 463
