m_matrix.c revision 23caf20169ac38436ee9c13914f1d6aa7cf6bb5e
1/* $Id: m_matrix.c,v 1.1 2000/11/16 21:05:41 keithw Exp $ */ 2 3/* 4 * Mesa 3-D graphics library 5 * Version: 3.5 6 * 7 * Copyright (C) 1999-2000 Brian Paul All Rights Reserved. 8 * 9 * Permission is hereby granted, free of charge, to any person obtaining a 10 * copy of this software and associated documentation files (the "Software"), 11 * to deal in the Software without restriction, including without limitation 12 * the rights to use, copy, modify, merge, publish, distribute, sublicense, 13 * and/or sell copies of the Software, and to permit persons to whom the 14 * Software is furnished to do so, subject to the following conditions: 15 * 16 * The above copyright notice and this permission notice shall be included 17 * in all copies or substantial portions of the Software. 18 * 19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 20 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 22 * BRIAN PAUL BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN 23 * AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 24 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 25 */ 26 27 28/* 29 * Matrix operations 30 * 31 * NOTES: 32 * 1. 4x4 transformation matrices are stored in memory in column major order. 33 * 2. Points/vertices are to be thought of as column vectors. 34 * 3. Transformation of a point p by a matrix M is: p' = M * p 35 */ 36 37 38#include "glheader.h" 39#include "macros.h" 40#include "mem.h" 41#include "mmath.h" 42 43#include "m_matrix.h" 44 45 46static const char *types[] = { 47 "MATRIX_GENERAL", 48 "MATRIX_IDENTITY", 49 "MATRIX_3D_NO_ROT", 50 "MATRIX_PERSPECTIVE", 51 "MATRIX_2D", 52 "MATRIX_2D_NO_ROT", 53 "MATRIX_3D" 54}; 55 56 57static GLfloat Identity[16] = { 58 1.0, 0.0, 0.0, 0.0, 59 0.0, 1.0, 0.0, 0.0, 60 0.0, 0.0, 1.0, 0.0, 61 0.0, 0.0, 0.0, 1.0 62}; 63 64 65 66 67/* 68 * This matmul was contributed by Thomas Malik 69 * 70 * Perform a 4x4 matrix multiplication (product = a x b). 71 * Input: a, b - matrices to multiply 72 * Output: product - product of a and b 73 * WARNING: (product != b) assumed 74 * NOTE: (product == a) allowed 75 * 76 * KW: 4*16 = 64 muls 77 */ 78#define A(row,col) a[(col<<2)+row] 79#define B(row,col) b[(col<<2)+row] 80#define P(row,col) product[(col<<2)+row] 81 82static void matmul4( GLfloat *product, const GLfloat *a, const GLfloat *b ) 83{ 84 GLint i; 85 for (i = 0; i < 4; i++) { 86 const GLfloat ai0=A(i,0), ai1=A(i,1), ai2=A(i,2), ai3=A(i,3); 87 P(i,0) = ai0 * B(0,0) + ai1 * B(1,0) + ai2 * B(2,0) + ai3 * B(3,0); 88 P(i,1) = ai0 * B(0,1) + ai1 * B(1,1) + ai2 * B(2,1) + ai3 * B(3,1); 89 P(i,2) = ai0 * B(0,2) + ai1 * B(1,2) + ai2 * B(2,2) + ai3 * B(3,2); 90 P(i,3) = ai0 * B(0,3) + ai1 * B(1,3) + ai2 * B(2,3) + ai3 * B(3,3); 91 } 92} 93 94 95/* Multiply two matrices known to occupy only the top three rows, such 96 * as typical model matrices, and ortho matrices. 97 */ 98static void matmul34( GLfloat *product, const GLfloat *a, const GLfloat *b ) 99{ 100 GLint i; 101 for (i = 0; i < 3; i++) { 102 const GLfloat ai0=A(i,0), ai1=A(i,1), ai2=A(i,2), ai3=A(i,3); 103 P(i,0) = ai0 * B(0,0) + ai1 * B(1,0) + ai2 * B(2,0); 104 P(i,1) = ai0 * B(0,1) + ai1 * B(1,1) + ai2 * B(2,1); 105 P(i,2) = ai0 * B(0,2) + ai1 * B(1,2) + ai2 * B(2,2); 106 P(i,3) = ai0 * B(0,3) + ai1 * B(1,3) + ai2 * B(2,3) + ai3; 107 } 108 P(3,0) = 0; 109 P(3,1) = 0; 110 P(3,2) = 0; 111 P(3,3) = 1; 112} 113 114 115#undef A 116#undef B 117#undef P 118 119 120/* 121 * Multiply a matrix by an array of floats with known properties. 122 */ 123static void matrix_multf( GLmatrix *mat, const GLfloat *m, GLuint flags ) 124{ 125 mat->flags |= (flags | MAT_DIRTY_TYPE | MAT_DIRTY_INVERSE); 126 127 if (TEST_MAT_FLAGS(mat, MAT_FLAGS_3D)) 128 matmul34( mat->m, mat->m, m ); 129 else 130 matmul4( mat->m, mat->m, m ); 131} 132 133 134static void print_matrix_floats( const GLfloat m[16] ) 135{ 136 int i; 137 for (i=0;i<4;i++) { 138 fprintf(stderr,"\t%f %f %f %f\n", m[i], m[4+i], m[8+i], m[12+i] ); 139 } 140} 141 142void 143_math_matrix_print( const GLmatrix *m ) 144{ 145 fprintf(stderr, "Matrix type: %s, flags: %x\n", types[m->type], m->flags); 146 print_matrix_floats(m->m); 147 fprintf(stderr, "Inverse: \n"); 148 if (m->inv) { 149 GLfloat prod[16]; 150 print_matrix_floats(m->inv); 151 matmul4(prod, m->m, m->inv); 152 fprintf(stderr, "Mat * Inverse:\n"); 153 print_matrix_floats(prod); 154 } 155 else { 156 fprintf(stderr, " - not available\n"); 157 } 158} 159 160 161 162 163#define SWAP_ROWS(a, b) { GLfloat *_tmp = a; (a)=(b); (b)=_tmp; } 164#define MAT(m,r,c) (m)[(c)*4+(r)] 165 166/* 167 * Compute inverse of 4x4 transformation matrix. 168 * Code contributed by Jacques Leroy jle@star.be 169 * Return GL_TRUE for success, GL_FALSE for failure (singular matrix) 170 */ 171static GLboolean invert_matrix_general( GLmatrix *mat ) 172{ 173 const GLfloat *m = mat->m; 174 GLfloat *out = mat->inv; 175 GLfloat wtmp[4][8]; 176 GLfloat m0, m1, m2, m3, s; 177 GLfloat *r0, *r1, *r2, *r3; 178 179 r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3]; 180 181 r0[0] = MAT(m,0,0), r0[1] = MAT(m,0,1), 182 r0[2] = MAT(m,0,2), r0[3] = MAT(m,0,3), 183 r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0, 184 185 r1[0] = MAT(m,1,0), r1[1] = MAT(m,1,1), 186 r1[2] = MAT(m,1,2), r1[3] = MAT(m,1,3), 187 r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0, 188 189 r2[0] = MAT(m,2,0), r2[1] = MAT(m,2,1), 190 r2[2] = MAT(m,2,2), r2[3] = MAT(m,2,3), 191 r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0, 192 193 r3[0] = MAT(m,3,0), r3[1] = MAT(m,3,1), 194 r3[2] = MAT(m,3,2), r3[3] = MAT(m,3,3), 195 r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0; 196 197 /* choose pivot - or die */ 198 if (fabs(r3[0])>fabs(r2[0])) SWAP_ROWS(r3, r2); 199 if (fabs(r2[0])>fabs(r1[0])) SWAP_ROWS(r2, r1); 200 if (fabs(r1[0])>fabs(r0[0])) SWAP_ROWS(r1, r0); 201 if (0.0 == r0[0]) return GL_FALSE; 202 203 /* eliminate first variable */ 204 m1 = r1[0]/r0[0]; m2 = r2[0]/r0[0]; m3 = r3[0]/r0[0]; 205 s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s; 206 s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s; 207 s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s; 208 s = r0[4]; 209 if (s != 0.0) { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; } 210 s = r0[5]; 211 if (s != 0.0) { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; } 212 s = r0[6]; 213 if (s != 0.0) { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; } 214 s = r0[7]; 215 if (s != 0.0) { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; } 216 217 /* choose pivot - or die */ 218 if (fabs(r3[1])>fabs(r2[1])) SWAP_ROWS(r3, r2); 219 if (fabs(r2[1])>fabs(r1[1])) SWAP_ROWS(r2, r1); 220 if (0.0 == r1[1]) return GL_FALSE; 221 222 /* eliminate second variable */ 223 m2 = r2[1]/r1[1]; m3 = r3[1]/r1[1]; 224 r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2]; 225 r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3]; 226 s = r1[4]; if (0.0 != s) { r2[4] -= m2 * s; r3[4] -= m3 * s; } 227 s = r1[5]; if (0.0 != s) { r2[5] -= m2 * s; r3[5] -= m3 * s; } 228 s = r1[6]; if (0.0 != s) { r2[6] -= m2 * s; r3[6] -= m3 * s; } 229 s = r1[7]; if (0.0 != s) { r2[7] -= m2 * s; r3[7] -= m3 * s; } 230 231 /* choose pivot - or die */ 232 if (fabs(r3[2])>fabs(r2[2])) SWAP_ROWS(r3, r2); 233 if (0.0 == r2[2]) return GL_FALSE; 234 235 /* eliminate third variable */ 236 m3 = r3[2]/r2[2]; 237 r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4], 238 r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6], 239 r3[7] -= m3 * r2[7]; 240 241 /* last check */ 242 if (0.0 == r3[3]) return GL_FALSE; 243 244 s = 1.0/r3[3]; /* now back substitute row 3 */ 245 r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s; 246 247 m2 = r2[3]; /* now back substitute row 2 */ 248 s = 1.0/r2[2]; 249 r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2), 250 r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2); 251 m1 = r1[3]; 252 r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1, 253 r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1; 254 m0 = r0[3]; 255 r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0, 256 r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0; 257 258 m1 = r1[2]; /* now back substitute row 1 */ 259 s = 1.0/r1[1]; 260 r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1), 261 r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1); 262 m0 = r0[2]; 263 r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0, 264 r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0; 265 266 m0 = r0[1]; /* now back substitute row 0 */ 267 s = 1.0/r0[0]; 268 r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0), 269 r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0); 270 271 MAT(out,0,0) = r0[4]; MAT(out,0,1) = r0[5], 272 MAT(out,0,2) = r0[6]; MAT(out,0,3) = r0[7], 273 MAT(out,1,0) = r1[4]; MAT(out,1,1) = r1[5], 274 MAT(out,1,2) = r1[6]; MAT(out,1,3) = r1[7], 275 MAT(out,2,0) = r2[4]; MAT(out,2,1) = r2[5], 276 MAT(out,2,2) = r2[6]; MAT(out,2,3) = r2[7], 277 MAT(out,3,0) = r3[4]; MAT(out,3,1) = r3[5], 278 MAT(out,3,2) = r3[6]; MAT(out,3,3) = r3[7]; 279 280 return GL_TRUE; 281} 282#undef SWAP_ROWS 283 284 285/* Adapted from graphics gems II. 286 */ 287static GLboolean invert_matrix_3d_general( GLmatrix *mat ) 288{ 289 const GLfloat *in = mat->m; 290 GLfloat *out = mat->inv; 291 GLfloat pos, neg, t; 292 GLfloat det; 293 294 /* Calculate the determinant of upper left 3x3 submatrix and 295 * determine if the matrix is singular. 296 */ 297 pos = neg = 0.0; 298 t = MAT(in,0,0) * MAT(in,1,1) * MAT(in,2,2); 299 if (t >= 0.0) pos += t; else neg += t; 300 301 t = MAT(in,1,0) * MAT(in,2,1) * MAT(in,0,2); 302 if (t >= 0.0) pos += t; else neg += t; 303 304 t = MAT(in,2,0) * MAT(in,0,1) * MAT(in,1,2); 305 if (t >= 0.0) pos += t; else neg += t; 306 307 t = -MAT(in,2,0) * MAT(in,1,1) * MAT(in,0,2); 308 if (t >= 0.0) pos += t; else neg += t; 309 310 t = -MAT(in,1,0) * MAT(in,0,1) * MAT(in,2,2); 311 if (t >= 0.0) pos += t; else neg += t; 312 313 t = -MAT(in,0,0) * MAT(in,2,1) * MAT(in,1,2); 314 if (t >= 0.0) pos += t; else neg += t; 315 316 det = pos + neg; 317 318 if (det*det < 1e-25) 319 return GL_FALSE; 320 321 det = 1.0 / det; 322 MAT(out,0,0) = ( (MAT(in,1,1)*MAT(in,2,2) - MAT(in,2,1)*MAT(in,1,2) )*det); 323 MAT(out,0,1) = (- (MAT(in,0,1)*MAT(in,2,2) - MAT(in,2,1)*MAT(in,0,2) )*det); 324 MAT(out,0,2) = ( (MAT(in,0,1)*MAT(in,1,2) - MAT(in,1,1)*MAT(in,0,2) )*det); 325 MAT(out,1,0) = (- (MAT(in,1,0)*MAT(in,2,2) - MAT(in,2,0)*MAT(in,1,2) )*det); 326 MAT(out,1,1) = ( (MAT(in,0,0)*MAT(in,2,2) - MAT(in,2,0)*MAT(in,0,2) )*det); 327 MAT(out,1,2) = (- (MAT(in,0,0)*MAT(in,1,2) - MAT(in,1,0)*MAT(in,0,2) )*det); 328 MAT(out,2,0) = ( (MAT(in,1,0)*MAT(in,2,1) - MAT(in,2,0)*MAT(in,1,1) )*det); 329 MAT(out,2,1) = (- (MAT(in,0,0)*MAT(in,2,1) - MAT(in,2,0)*MAT(in,0,1) )*det); 330 MAT(out,2,2) = ( (MAT(in,0,0)*MAT(in,1,1) - MAT(in,1,0)*MAT(in,0,1) )*det); 331 332 /* Do the translation part */ 333 MAT(out,0,3) = - (MAT(in,0,3) * MAT(out,0,0) + 334 MAT(in,1,3) * MAT(out,0,1) + 335 MAT(in,2,3) * MAT(out,0,2) ); 336 MAT(out,1,3) = - (MAT(in,0,3) * MAT(out,1,0) + 337 MAT(in,1,3) * MAT(out,1,1) + 338 MAT(in,2,3) * MAT(out,1,2) ); 339 MAT(out,2,3) = - (MAT(in,0,3) * MAT(out,2,0) + 340 MAT(in,1,3) * MAT(out,2,1) + 341 MAT(in,2,3) * MAT(out,2,2) ); 342 343 return GL_TRUE; 344} 345 346 347static GLboolean invert_matrix_3d( GLmatrix *mat ) 348{ 349 const GLfloat *in = mat->m; 350 GLfloat *out = mat->inv; 351 352 if (!TEST_MAT_FLAGS(mat, MAT_FLAGS_ANGLE_PRESERVING)) { 353 return invert_matrix_3d_general( mat ); 354 } 355 356 if (mat->flags & MAT_FLAG_UNIFORM_SCALE) { 357 GLfloat scale = (MAT(in,0,0) * MAT(in,0,0) + 358 MAT(in,0,1) * MAT(in,0,1) + 359 MAT(in,0,2) * MAT(in,0,2)); 360 361 if (scale == 0.0) 362 return GL_FALSE; 363 364 scale = 1.0 / scale; 365 366 /* Transpose and scale the 3 by 3 upper-left submatrix. */ 367 MAT(out,0,0) = scale * MAT(in,0,0); 368 MAT(out,1,0) = scale * MAT(in,0,1); 369 MAT(out,2,0) = scale * MAT(in,0,2); 370 MAT(out,0,1) = scale * MAT(in,1,0); 371 MAT(out,1,1) = scale * MAT(in,1,1); 372 MAT(out,2,1) = scale * MAT(in,1,2); 373 MAT(out,0,2) = scale * MAT(in,2,0); 374 MAT(out,1,2) = scale * MAT(in,2,1); 375 MAT(out,2,2) = scale * MAT(in,2,2); 376 } 377 else if (mat->flags & MAT_FLAG_ROTATION) { 378 /* Transpose the 3 by 3 upper-left submatrix. */ 379 MAT(out,0,0) = MAT(in,0,0); 380 MAT(out,1,0) = MAT(in,0,1); 381 MAT(out,2,0) = MAT(in,0,2); 382 MAT(out,0,1) = MAT(in,1,0); 383 MAT(out,1,1) = MAT(in,1,1); 384 MAT(out,2,1) = MAT(in,1,2); 385 MAT(out,0,2) = MAT(in,2,0); 386 MAT(out,1,2) = MAT(in,2,1); 387 MAT(out,2,2) = MAT(in,2,2); 388 } 389 else { 390 /* pure translation */ 391 MEMCPY( out, Identity, sizeof(Identity) ); 392 MAT(out,0,3) = - MAT(in,0,3); 393 MAT(out,1,3) = - MAT(in,1,3); 394 MAT(out,2,3) = - MAT(in,2,3); 395 return GL_TRUE; 396 } 397 398 if (mat->flags & MAT_FLAG_TRANSLATION) { 399 /* Do the translation part */ 400 MAT(out,0,3) = - (MAT(in,0,3) * MAT(out,0,0) + 401 MAT(in,1,3) * MAT(out,0,1) + 402 MAT(in,2,3) * MAT(out,0,2) ); 403 MAT(out,1,3) = - (MAT(in,0,3) * MAT(out,1,0) + 404 MAT(in,1,3) * MAT(out,1,1) + 405 MAT(in,2,3) * MAT(out,1,2) ); 406 MAT(out,2,3) = - (MAT(in,0,3) * MAT(out,2,0) + 407 MAT(in,1,3) * MAT(out,2,1) + 408 MAT(in,2,3) * MAT(out,2,2) ); 409 } 410 else { 411 MAT(out,0,3) = MAT(out,1,3) = MAT(out,2,3) = 0.0; 412 } 413 414 return GL_TRUE; 415} 416 417 418 419static GLboolean invert_matrix_identity( GLmatrix *mat ) 420{ 421 MEMCPY( mat->inv, Identity, sizeof(Identity) ); 422 return GL_TRUE; 423} 424 425 426static GLboolean invert_matrix_3d_no_rot( GLmatrix *mat ) 427{ 428 const GLfloat *in = mat->m; 429 GLfloat *out = mat->inv; 430 431 if (MAT(in,0,0) == 0 || MAT(in,1,1) == 0 || MAT(in,2,2) == 0 ) 432 return GL_FALSE; 433 434 MEMCPY( out, Identity, 16 * sizeof(GLfloat) ); 435 MAT(out,0,0) = 1.0 / MAT(in,0,0); 436 MAT(out,1,1) = 1.0 / MAT(in,1,1); 437 MAT(out,2,2) = 1.0 / MAT(in,2,2); 438 439 if (mat->flags & MAT_FLAG_TRANSLATION) { 440 MAT(out,0,3) = - (MAT(in,0,3) * MAT(out,0,0)); 441 MAT(out,1,3) = - (MAT(in,1,3) * MAT(out,1,1)); 442 MAT(out,2,3) = - (MAT(in,2,3) * MAT(out,2,2)); 443 } 444 445 return GL_TRUE; 446} 447 448 449static GLboolean invert_matrix_2d_no_rot( GLmatrix *mat ) 450{ 451 const GLfloat *in = mat->m; 452 GLfloat *out = mat->inv; 453 454 if (MAT(in,0,0) == 0 || MAT(in,1,1) == 0) 455 return GL_FALSE; 456 457 MEMCPY( out, Identity, 16 * sizeof(GLfloat) ); 458 MAT(out,0,0) = 1.0 / MAT(in,0,0); 459 MAT(out,1,1) = 1.0 / MAT(in,1,1); 460 461 if (mat->flags & MAT_FLAG_TRANSLATION) { 462 MAT(out,0,3) = - (MAT(in,0,3) * MAT(out,0,0)); 463 MAT(out,1,3) = - (MAT(in,1,3) * MAT(out,1,1)); 464 } 465 466 return GL_TRUE; 467} 468 469 470static GLboolean invert_matrix_perspective( GLmatrix *mat ) 471{ 472 const GLfloat *in = mat->m; 473 GLfloat *out = mat->inv; 474 475 if (MAT(in,2,3) == 0) 476 return GL_FALSE; 477 478 MEMCPY( out, Identity, 16 * sizeof(GLfloat) ); 479 480 MAT(out,0,0) = 1.0 / MAT(in,0,0); 481 MAT(out,1,1) = 1.0 / MAT(in,1,1); 482 483 MAT(out,0,3) = MAT(in,0,2); 484 MAT(out,1,3) = MAT(in,1,2); 485 486 MAT(out,2,2) = 0; 487 MAT(out,2,3) = -1; 488 489 MAT(out,3,2) = 1.0 / MAT(in,2,3); 490 MAT(out,3,3) = MAT(in,2,2) * MAT(out,3,2); 491 492 return GL_TRUE; 493} 494 495 496typedef GLboolean (*inv_mat_func)( GLmatrix *mat ); 497 498 499static inv_mat_func inv_mat_tab[7] = { 500 invert_matrix_general, 501 invert_matrix_identity, 502 invert_matrix_3d_no_rot, 503 invert_matrix_perspective, 504 invert_matrix_3d, /* lazy! */ 505 invert_matrix_2d_no_rot, 506 invert_matrix_3d 507}; 508 509 510static GLboolean matrix_invert( GLmatrix *mat ) 511{ 512 if (inv_mat_tab[mat->type](mat)) { 513 mat->flags &= ~MAT_FLAG_SINGULAR; 514 return GL_TRUE; 515 } else { 516 mat->flags |= MAT_FLAG_SINGULAR; 517 MEMCPY( mat->inv, Identity, sizeof(Identity) ); 518 return GL_FALSE; 519 } 520} 521 522 523 524 525 526 527/* 528 * Generate a 4x4 transformation matrix from glRotate parameters, and 529 * postmultiply the input matrix by it. 530 */ 531void 532_math_matrix_rotate( GLmatrix *mat, 533 GLfloat angle, GLfloat x, GLfloat y, GLfloat z ) 534{ 535 /* This function contributed by Erich Boleyn (erich@uruk.org) */ 536 GLfloat mag, s, c; 537 GLfloat xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c; 538 GLfloat m[16]; 539 540 s = sin( angle * DEG2RAD ); 541 c = cos( angle * DEG2RAD ); 542 543 mag = GL_SQRT( x*x + y*y + z*z ); 544 545 if (mag <= 1.0e-4) { 546 /* generate an identity matrix and return */ 547 MEMCPY(m, Identity, sizeof(GLfloat)*16); 548 return; 549 } 550 551 x /= mag; 552 y /= mag; 553 z /= mag; 554 555#define M(row,col) m[col*4+row] 556 557 /* 558 * Arbitrary axis rotation matrix. 559 * 560 * This is composed of 5 matrices, Rz, Ry, T, Ry', Rz', multiplied 561 * like so: Rz * Ry * T * Ry' * Rz'. T is the final rotation 562 * (which is about the X-axis), and the two composite transforms 563 * Ry' * Rz' and Rz * Ry are (respectively) the rotations necessary 564 * from the arbitrary axis to the X-axis then back. They are 565 * all elementary rotations. 566 * 567 * Rz' is a rotation about the Z-axis, to bring the axis vector 568 * into the x-z plane. Then Ry' is applied, rotating about the 569 * Y-axis to bring the axis vector parallel with the X-axis. The 570 * rotation about the X-axis is then performed. Ry and Rz are 571 * simply the respective inverse transforms to bring the arbitrary 572 * axis back to it's original orientation. The first transforms 573 * Rz' and Ry' are considered inverses, since the data from the 574 * arbitrary axis gives you info on how to get to it, not how 575 * to get away from it, and an inverse must be applied. 576 * 577 * The basic calculation used is to recognize that the arbitrary 578 * axis vector (x, y, z), since it is of unit length, actually 579 * represents the sines and cosines of the angles to rotate the 580 * X-axis to the same orientation, with theta being the angle about 581 * Z and phi the angle about Y (in the order described above) 582 * as follows: 583 * 584 * cos ( theta ) = x / sqrt ( 1 - z^2 ) 585 * sin ( theta ) = y / sqrt ( 1 - z^2 ) 586 * 587 * cos ( phi ) = sqrt ( 1 - z^2 ) 588 * sin ( phi ) = z 589 * 590 * Note that cos ( phi ) can further be inserted to the above 591 * formulas: 592 * 593 * cos ( theta ) = x / cos ( phi ) 594 * sin ( theta ) = y / sin ( phi ) 595 * 596 * ...etc. Because of those relations and the standard trigonometric 597 * relations, it is pssible to reduce the transforms down to what 598 * is used below. It may be that any primary axis chosen will give the 599 * same results (modulo a sign convention) using thie method. 600 * 601 * Particularly nice is to notice that all divisions that might 602 * have caused trouble when parallel to certain planes or 603 * axis go away with care paid to reducing the expressions. 604 * After checking, it does perform correctly under all cases, since 605 * in all the cases of division where the denominator would have 606 * been zero, the numerator would have been zero as well, giving 607 * the expected result. 608 */ 609 610 xx = x * x; 611 yy = y * y; 612 zz = z * z; 613 xy = x * y; 614 yz = y * z; 615 zx = z * x; 616 xs = x * s; 617 ys = y * s; 618 zs = z * s; 619 one_c = 1.0F - c; 620 621 M(0,0) = (one_c * xx) + c; 622 M(0,1) = (one_c * xy) - zs; 623 M(0,2) = (one_c * zx) + ys; 624 M(0,3) = 0.0F; 625 626 M(1,0) = (one_c * xy) + zs; 627 M(1,1) = (one_c * yy) + c; 628 M(1,2) = (one_c * yz) - xs; 629 M(1,3) = 0.0F; 630 631 M(2,0) = (one_c * zx) - ys; 632 M(2,1) = (one_c * yz) + xs; 633 M(2,2) = (one_c * zz) + c; 634 M(2,3) = 0.0F; 635 636 M(3,0) = 0.0F; 637 M(3,1) = 0.0F; 638 M(3,2) = 0.0F; 639 M(3,3) = 1.0F; 640 641#undef M 642 643 matrix_multf( mat, m, MAT_FLAG_ROTATION ); 644} 645 646 647void 648_math_matrix_frustrum( GLmatrix *mat, 649 GLfloat left, GLfloat right, 650 GLfloat bottom, GLfloat top, 651 GLfloat nearval, GLfloat farval ) 652{ 653 GLfloat x, y, a, b, c, d; 654 GLfloat m[16]; 655 656 x = (2.0*nearval) / (right-left); 657 y = (2.0*nearval) / (top-bottom); 658 a = (right+left) / (right-left); 659 b = (top+bottom) / (top-bottom); 660 c = -(farval+nearval) / ( farval-nearval); 661 d = -(2.0*farval*nearval) / (farval-nearval); /* error? */ 662 663#define M(row,col) m[col*4+row] 664 M(0,0) = x; M(0,1) = 0.0F; M(0,2) = a; M(0,3) = 0.0F; 665 M(1,0) = 0.0F; M(1,1) = y; M(1,2) = b; M(1,3) = 0.0F; 666 M(2,0) = 0.0F; M(2,1) = 0.0F; M(2,2) = c; M(2,3) = d; 667 M(3,0) = 0.0F; M(3,1) = 0.0F; M(3,2) = -1.0F; M(3,3) = 0.0F; 668#undef M 669 670 matrix_multf( mat, m, MAT_FLAG_PERSPECTIVE ); 671} 672 673void 674_math_matrix_ortho( GLmatrix *mat, 675 GLfloat left, GLfloat right, 676 GLfloat bottom, GLfloat top, 677 GLfloat nearval, GLfloat farval ) 678{ 679 GLfloat x, y, z; 680 GLfloat tx, ty, tz; 681 GLfloat m[16]; 682 683 x = 2.0 / (right-left); 684 y = 2.0 / (top-bottom); 685 z = -2.0 / (farval-nearval); 686 tx = -(right+left) / (right-left); 687 ty = -(top+bottom) / (top-bottom); 688 tz = -(farval+nearval) / (farval-nearval); 689 690#define M(row,col) m[col*4+row] 691 M(0,0) = x; M(0,1) = 0.0F; M(0,2) = 0.0F; M(0,3) = tx; 692 M(1,0) = 0.0F; M(1,1) = y; M(1,2) = 0.0F; M(1,3) = ty; 693 M(2,0) = 0.0F; M(2,1) = 0.0F; M(2,2) = z; M(2,3) = tz; 694 M(3,0) = 0.0F; M(3,1) = 0.0F; M(3,2) = 0.0F; M(3,3) = 1.0F; 695#undef M 696 697 matrix_multf( mat, m, (MAT_FLAG_GENERAL_SCALE|MAT_FLAG_TRANSLATION)); 698} 699 700 701#define ZERO(x) (1<<x) 702#define ONE(x) (1<<(x+16)) 703 704#define MASK_NO_TRX (ZERO(12) | ZERO(13) | ZERO(14)) 705#define MASK_NO_2D_SCALE ( ONE(0) | ONE(5)) 706 707#define MASK_IDENTITY ( ONE(0) | ZERO(4) | ZERO(8) | ZERO(12) |\ 708 ZERO(1) | ONE(5) | ZERO(9) | ZERO(13) |\ 709 ZERO(2) | ZERO(6) | ONE(10) | ZERO(14) |\ 710 ZERO(3) | ZERO(7) | ZERO(11) | ONE(15) ) 711 712#define MASK_2D_NO_ROT ( ZERO(4) | ZERO(8) | \ 713 ZERO(1) | ZERO(9) | \ 714 ZERO(2) | ZERO(6) | ONE(10) | ZERO(14) |\ 715 ZERO(3) | ZERO(7) | ZERO(11) | ONE(15) ) 716 717#define MASK_2D ( ZERO(8) | \ 718 ZERO(9) | \ 719 ZERO(2) | ZERO(6) | ONE(10) | ZERO(14) |\ 720 ZERO(3) | ZERO(7) | ZERO(11) | ONE(15) ) 721 722 723#define MASK_3D_NO_ROT ( ZERO(4) | ZERO(8) | \ 724 ZERO(1) | ZERO(9) | \ 725 ZERO(2) | ZERO(6) | \ 726 ZERO(3) | ZERO(7) | ZERO(11) | ONE(15) ) 727 728#define MASK_3D ( \ 729 \ 730 \ 731 ZERO(3) | ZERO(7) | ZERO(11) | ONE(15) ) 732 733 734#define MASK_PERSPECTIVE ( ZERO(4) | ZERO(12) |\ 735 ZERO(1) | ZERO(13) |\ 736 ZERO(2) | ZERO(6) | \ 737 ZERO(3) | ZERO(7) | ZERO(15) ) 738 739#define SQ(x) ((x)*(x)) 740 741/* Determine type and flags from scratch. This is expensive enough to 742 * only want to do it once. 743 */ 744static void analyze_from_scratch( GLmatrix *mat ) 745{ 746 const GLfloat *m = mat->m; 747 GLuint mask = 0; 748 GLuint i; 749 750 for (i = 0 ; i < 16 ; i++) { 751 if (m[i] == 0.0) mask |= (1<<i); 752 } 753 754 if (m[0] == 1.0F) mask |= (1<<16); 755 if (m[5] == 1.0F) mask |= (1<<21); 756 if (m[10] == 1.0F) mask |= (1<<26); 757 if (m[15] == 1.0F) mask |= (1<<31); 758 759 mat->flags &= ~MAT_FLAGS_GEOMETRY; 760 761 /* Check for translation - no-one really cares 762 */ 763 if ((mask & MASK_NO_TRX) != MASK_NO_TRX) 764 mat->flags |= MAT_FLAG_TRANSLATION; 765 766 /* Do the real work 767 */ 768 if (mask == MASK_IDENTITY) { 769 mat->type = MATRIX_IDENTITY; 770 } 771 else if ((mask & MASK_2D_NO_ROT) == MASK_2D_NO_ROT) { 772 mat->type = MATRIX_2D_NO_ROT; 773 774 if ((mask & MASK_NO_2D_SCALE) != MASK_NO_2D_SCALE) 775 mat->flags = MAT_FLAG_GENERAL_SCALE; 776 } 777 else if ((mask & MASK_2D) == MASK_2D) { 778 GLfloat mm = DOT2(m, m); 779 GLfloat m4m4 = DOT2(m+4,m+4); 780 GLfloat mm4 = DOT2(m,m+4); 781 782 mat->type = MATRIX_2D; 783 784 /* Check for scale */ 785 if (SQ(mm-1) > SQ(1e-6) || 786 SQ(m4m4-1) > SQ(1e-6)) 787 mat->flags |= MAT_FLAG_GENERAL_SCALE; 788 789 /* Check for rotation */ 790 if (SQ(mm4) > SQ(1e-6)) 791 mat->flags |= MAT_FLAG_GENERAL_3D; 792 else 793 mat->flags |= MAT_FLAG_ROTATION; 794 795 } 796 else if ((mask & MASK_3D_NO_ROT) == MASK_3D_NO_ROT) { 797 mat->type = MATRIX_3D_NO_ROT; 798 799 /* Check for scale */ 800 if (SQ(m[0]-m[5]) < SQ(1e-6) && 801 SQ(m[0]-m[10]) < SQ(1e-6)) { 802 if (SQ(m[0]-1.0) > SQ(1e-6)) { 803 mat->flags |= MAT_FLAG_UNIFORM_SCALE; 804 } 805 } 806 else { 807 mat->flags |= MAT_FLAG_GENERAL_SCALE; 808 } 809 } 810 else if ((mask & MASK_3D) == MASK_3D) { 811 GLfloat c1 = DOT3(m,m); 812 GLfloat c2 = DOT3(m+4,m+4); 813 GLfloat c3 = DOT3(m+8,m+8); 814 GLfloat d1 = DOT3(m, m+4); 815 GLfloat cp[3]; 816 817 mat->type = MATRIX_3D; 818 819 /* Check for scale */ 820 if (SQ(c1-c2) < SQ(1e-6) && SQ(c1-c3) < SQ(1e-6)) { 821 if (SQ(c1-1.0) > SQ(1e-6)) 822 mat->flags |= MAT_FLAG_UNIFORM_SCALE; 823 /* else no scale at all */ 824 } 825 else { 826 mat->flags |= MAT_FLAG_GENERAL_SCALE; 827 } 828 829 /* Check for rotation */ 830 if (SQ(d1) < SQ(1e-6)) { 831 CROSS3( cp, m, m+4 ); 832 SUB_3V( cp, cp, (m+8) ); 833 if (LEN_SQUARED_3FV(cp) < SQ(1e-6)) 834 mat->flags |= MAT_FLAG_ROTATION; 835 else 836 mat->flags |= MAT_FLAG_GENERAL_3D; 837 } 838 else { 839 mat->flags |= MAT_FLAG_GENERAL_3D; /* shear, etc */ 840 } 841 } 842 else if ((mask & MASK_PERSPECTIVE) == MASK_PERSPECTIVE && m[11]==-1.0F) { 843 mat->type = MATRIX_PERSPECTIVE; 844 mat->flags |= MAT_FLAG_GENERAL; 845 } 846 else { 847 mat->type = MATRIX_GENERAL; 848 mat->flags |= MAT_FLAG_GENERAL; 849 } 850} 851 852 853/* Analyse a matrix given that its flags are accurate - this is the 854 * more common operation, hopefully. 855 */ 856static void analyze_from_flags( GLmatrix *mat ) 857{ 858 const GLfloat *m = mat->m; 859 860 if (TEST_MAT_FLAGS(mat, 0)) { 861 mat->type = MATRIX_IDENTITY; 862 } 863 else if (TEST_MAT_FLAGS(mat, (MAT_FLAG_TRANSLATION | 864 MAT_FLAG_UNIFORM_SCALE | 865 MAT_FLAG_GENERAL_SCALE))) { 866 if ( m[10]==1.0F && m[14]==0.0F ) { 867 mat->type = MATRIX_2D_NO_ROT; 868 } 869 else { 870 mat->type = MATRIX_3D_NO_ROT; 871 } 872 } 873 else if (TEST_MAT_FLAGS(mat, MAT_FLAGS_3D)) { 874 if ( m[ 8]==0.0F 875 && m[ 9]==0.0F 876 && m[2]==0.0F && m[6]==0.0F && m[10]==1.0F && m[14]==0.0F) { 877 mat->type = MATRIX_2D; 878 } 879 else { 880 mat->type = MATRIX_3D; 881 } 882 } 883 else if ( m[4]==0.0F && m[12]==0.0F 884 && m[1]==0.0F && m[13]==0.0F 885 && m[2]==0.0F && m[6]==0.0F 886 && m[3]==0.0F && m[7]==0.0F && m[11]==-1.0F && m[15]==0.0F) { 887 mat->type = MATRIX_PERSPECTIVE; 888 } 889 else { 890 mat->type = MATRIX_GENERAL; 891 } 892} 893 894 895void 896_math_matrix_analyze( GLmatrix *mat ) 897{ 898 if (mat->flags & MAT_DIRTY_TYPE) { 899 if (mat->flags & MAT_DIRTY_FLAGS) 900 analyze_from_scratch( mat ); 901 else 902 analyze_from_flags( mat ); 903 } 904 905 if (mat->inv && (mat->flags & MAT_DIRTY_INVERSE)) { 906 matrix_invert( mat ); 907 } 908 909 mat->flags &= ~(MAT_DIRTY_FLAGS| 910 MAT_DIRTY_TYPE| 911 MAT_DIRTY_INVERSE); 912} 913 914 915void 916_math_matrix_copy( GLmatrix *to, const GLmatrix *from ) 917{ 918 MEMCPY( to->m, from->m, sizeof(Identity) ); 919 to->flags = from->flags; 920 to->type = from->type; 921 922 if (to->inv != 0) { 923 if (from->inv == 0) { 924 matrix_invert( to ); 925 } 926 else { 927 MEMCPY(to->inv, from->inv, sizeof(GLfloat)*16); 928 } 929 } 930} 931 932 933void 934_math_matrix_scale( GLmatrix *mat, GLfloat x, GLfloat y, GLfloat z ) 935{ 936 GLfloat *m = mat->m; 937 m[0] *= x; m[4] *= y; m[8] *= z; 938 m[1] *= x; m[5] *= y; m[9] *= z; 939 m[2] *= x; m[6] *= y; m[10] *= z; 940 m[3] *= x; m[7] *= y; m[11] *= z; 941 942 if (fabs(x - y) < 1e-8 && fabs(x - z) < 1e-8) 943 mat->flags |= MAT_FLAG_UNIFORM_SCALE; 944 else 945 mat->flags |= MAT_FLAG_GENERAL_SCALE; 946 947 mat->flags |= (MAT_DIRTY_TYPE | 948 MAT_DIRTY_INVERSE); 949} 950 951 952void 953_math_matrix_translate( GLmatrix *mat, GLfloat x, GLfloat y, GLfloat z ) 954{ 955 GLfloat *m = mat->m; 956 m[12] = m[0] * x + m[4] * y + m[8] * z + m[12]; 957 m[13] = m[1] * x + m[5] * y + m[9] * z + m[13]; 958 m[14] = m[2] * x + m[6] * y + m[10] * z + m[14]; 959 m[15] = m[3] * x + m[7] * y + m[11] * z + m[15]; 960 961 mat->flags |= (MAT_FLAG_TRANSLATION | 962 MAT_DIRTY_TYPE | 963 MAT_DIRTY_INVERSE); 964} 965 966 967void 968_math_matrix_loadf( GLmatrix *mat, const GLfloat *m ) 969{ 970 MEMCPY( mat->m, m, 16*sizeof(GLfloat) ); 971 mat->flags = (MAT_FLAG_GENERAL | MAT_DIRTY); 972} 973 974void 975_math_matrix_ctr( GLmatrix *m ) 976{ 977 if ( m->m == 0 ) { 978 m->m = (GLfloat *) ALIGN_MALLOC( 16 * sizeof(GLfloat), 16 ); 979 } 980 MEMCPY( m->m, Identity, sizeof(Identity) ); 981 m->inv = 0; 982 m->type = MATRIX_IDENTITY; 983 m->flags = 0; 984} 985 986void 987_math_matrix_dtr( GLmatrix *m ) 988{ 989 if ( m->m != 0 ) { 990 ALIGN_FREE( m->m ); 991 m->m = 0; 992 } 993 if ( m->inv != 0 ) { 994 ALIGN_FREE( m->inv ); 995 m->inv = 0; 996 } 997} 998 999 1000void 1001_math_matrix_alloc_inv( GLmatrix *m ) 1002{ 1003 if ( m->inv == 0 ) { 1004 m->inv = (GLfloat *) ALIGN_MALLOC( 16 * sizeof(GLfloat), 16 ); 1005 MEMCPY( m->inv, Identity, 16 * sizeof(GLfloat) ); 1006 } 1007} 1008 1009 1010void 1011_math_matrix_mul_matrix( GLmatrix *dest, const GLmatrix *a, const GLmatrix *b ) 1012{ 1013 dest->flags = (a->flags | 1014 b->flags | 1015 MAT_DIRTY_TYPE | 1016 MAT_DIRTY_INVERSE); 1017 1018 if (TEST_MAT_FLAGS(dest, MAT_FLAGS_3D)) 1019 matmul34( dest->m, a->m, b->m ); 1020 else 1021 matmul4( dest->m, a->m, b->m ); 1022} 1023 1024 1025void 1026_math_matrix_mul_floats( GLmatrix *dest, const GLfloat *m ) 1027{ 1028 dest->flags |= (MAT_FLAG_GENERAL | 1029 MAT_DIRTY_TYPE | 1030 MAT_DIRTY_INVERSE); 1031 1032 matmul4( dest->m, dest->m, m ); 1033} 1034 1035void 1036_math_matrix_set_identity( GLmatrix *mat ) 1037{ 1038 MEMCPY( mat->m, Identity, 16*sizeof(GLfloat) ); 1039 1040 if (mat->inv) 1041 MEMCPY( mat->inv, Identity, 16*sizeof(GLfloat) ); 1042 1043 mat->type = MATRIX_IDENTITY; 1044 mat->flags &= ~(MAT_DIRTY_FLAGS| 1045 MAT_DIRTY_TYPE| 1046 MAT_DIRTY_INVERSE); 1047} 1048 1049 1050 1051void 1052_math_transposef( GLfloat to[16], const GLfloat from[16] ) 1053{ 1054 to[0] = from[0]; 1055 to[1] = from[4]; 1056 to[2] = from[8]; 1057 to[3] = from[12]; 1058 to[4] = from[1]; 1059 to[5] = from[5]; 1060 to[6] = from[9]; 1061 to[7] = from[13]; 1062 to[8] = from[2]; 1063 to[9] = from[6]; 1064 to[10] = from[10]; 1065 to[11] = from[14]; 1066 to[12] = from[3]; 1067 to[13] = from[7]; 1068 to[14] = from[11]; 1069 to[15] = from[15]; 1070} 1071 1072 1073void 1074_math_transposed( GLdouble to[16], const GLdouble from[16] ) 1075{ 1076 to[0] = from[0]; 1077 to[1] = from[4]; 1078 to[2] = from[8]; 1079 to[3] = from[12]; 1080 to[4] = from[1]; 1081 to[5] = from[5]; 1082 to[6] = from[9]; 1083 to[7] = from[13]; 1084 to[8] = from[2]; 1085 to[9] = from[6]; 1086 to[10] = from[10]; 1087 to[11] = from[14]; 1088 to[12] = from[3]; 1089 to[13] = from[7]; 1090 to[14] = from[11]; 1091 to[15] = from[15]; 1092} 1093 1094void 1095_math_transposefd( GLfloat to[16], const GLdouble from[16] ) 1096{ 1097 to[0] = from[0]; 1098 to[1] = from[4]; 1099 to[2] = from[8]; 1100 to[3] = from[12]; 1101 to[4] = from[1]; 1102 to[5] = from[5]; 1103 to[6] = from[9]; 1104 to[7] = from[13]; 1105 to[8] = from[2]; 1106 to[9] = from[6]; 1107 to[10] = from[10]; 1108 to[11] = from[14]; 1109 to[12] = from[3]; 1110 to[13] = from[7]; 1111 to[14] = from[11]; 1112 to[15] = from[15]; 1113} 1114