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