src/mesa/math/m_matrix.c

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