libs/mathlib/m4x4.c

   1 /*
   2    Copyright (C) 1999-2007 id Software, Inc. and contributors.
   3    For a list of contributors, see the accompanying CONTRIBUTORS file.
   4
   5    This file is part of GtkRadiant.
   6
   7    GtkRadiant is free software; you can redistribute it and/or modify
   8    it under the terms of the GNU General Public License as published by
   9    the Free Software Foundation; either version 2 of the License, or
  10    (at your option) any later version.
  11
  12    GtkRadiant is distributed in the hope that it will be useful,
  13    but WITHOUT ANY WARRANTY; without even the implied warranty of
  14    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15    GNU General Public License for more details.
  16
  17    You should have received a copy of the GNU General Public License
  18    along with GtkRadiant; if not, write to the Free Software
  19    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  20  */
  21
  22 #include "mathlib.h"
  23 #include "memory.h"
  24
  25 void m4x4_identity( m4x4_t matrix ){
  26         matrix[1] = matrix[2] = matrix[3] =
  27                                                                 matrix[4] = matrix[6] = matrix[7] =
  28                                                                                                                         matrix[8] = matrix[9] = matrix[11] =
  29                                                                                                                                                                                 matrix[12] = matrix[13] = matrix[14] = 0;
  30
  31         matrix[0] = matrix[5] = matrix[10] = matrix[15] = 1;
  32 }
  33
  34 void m4x4_translation_for_vec3( m4x4_t matrix, const vec3_t translation ){
  35         matrix[1] = matrix[2] = matrix[3] =
  36                                                                 matrix[4] = matrix[6] = matrix[7] =
  37                                                                                                                         matrix[8] = matrix[9] = matrix[11] = 0;
  38
  39         matrix[0] = matrix[5] = matrix[10] = matrix[15] = 1;
  40
  41         matrix[12] = translation[0];
  42         matrix[13] = translation[1];
  43         matrix[14] = translation[2];
  44 }
  45
  46 void m4x4_rotation_for_vec3( m4x4_t matrix, const vec3_t euler, eulerOrder_t order ){
  47         double cx, sx, cy, sy, cz, sz;
  48
  49         cx = cos( DEG2RAD( euler[0] ) );
  50         sx = sin( DEG2RAD( euler[0] ) );
  51         cy = cos( DEG2RAD( euler[1] ) );
  52         sy = sin( DEG2RAD( euler[1] ) );
  53         cz = cos( DEG2RAD( euler[2] ) );
  54         sz = sin( DEG2RAD( euler[2] ) );
  55
  56         switch ( order )
  57         {
  58         case eXYZ:
  59
  60 #if 1
  61
  62                 {
  63                         matrix[0]  = (vec_t)( cy * cz );
  64                         matrix[1]  = (vec_t)( cy * sz );
  65                         matrix[2]  = (vec_t)-sy;
  66                         matrix[4]  = (vec_t)( sx * sy * cz + cx * -sz );
  67                         matrix[5]  = (vec_t)( sx * sy * sz + cx * cz );
  68                         matrix[6]  = (vec_t)( sx * cy );
  69                         matrix[8]  = (vec_t)( cx * sy * cz + sx * sz );
  70                         matrix[9]  = (vec_t)( cx * sy * sz + -sx * cz );
  71                         matrix[10] = (vec_t)( cx * cy );
  72                 }
  73
  74                 matrix[12]  =  matrix[13] = matrix[14] = matrix[3] = matrix[7] = matrix[11] = 0;
  75                 matrix[15] =  1;
  76
  77 #else
  78
  79                 m4x4_identity( matrix );
  80                 matrix[5] = (vec_t) cx; matrix[6] = (vec_t) sx;
  81                 matrix[9] = (vec_t)-sx; matrix[10] = (vec_t) cx;
  82
  83                 {
  84                         m4x4_t temp;
  85                         m4x4_identity( temp );
  86                         temp[0] = (vec_t) cy; temp[2] = (vec_t)-sy;
  87                         temp[8] = (vec_t) sy; temp[10] = (vec_t) cy;
  88                         m4x4_premultiply_by_m4x4( matrix, temp );
  89                         m4x4_identity( temp );
  90                         temp[0] = (vec_t) cz; temp[1] = (vec_t) sz;
  91                         temp[4] = (vec_t)-sz; temp[5] = (vec_t) cz;
  92                         m4x4_premultiply_by_m4x4( matrix, temp );
  93                 }
  94 #endif
  95
  96                 break;
  97
  98         case eYZX:
  99                 m4x4_identity( matrix );
 100                 matrix[0] = (vec_t) cy; matrix[2] = (vec_t)-sy;
 101                 matrix[8] = (vec_t) sy; matrix[10] = (vec_t) cy;
 102
 103                 {
 104                         m4x4_t temp;
 105                         m4x4_identity( temp );
 106                         temp[5] = (vec_t) cx; temp[6] = (vec_t) sx;
 107                         temp[9] = (vec_t)-sx; temp[10] = (vec_t) cx;
 108                         m4x4_premultiply_by_m4x4( matrix, temp );
 109                         m4x4_identity( temp );
 110                         temp[0] = (vec_t) cz; temp[1] = (vec_t) sz;
 111                         temp[4] = (vec_t)-sz; temp[5] = (vec_t) cz;
 112                         m4x4_premultiply_by_m4x4( matrix, temp );
 113                 }
 114                 break;
 115
 116         case eZXY:
 117                 m4x4_identity( matrix );
 118                 matrix[0] = (vec_t) cz; matrix[1] = (vec_t) sz;
 119                 matrix[4] = (vec_t)-sz; matrix[5] = (vec_t) cz;
 120
 121                 {
 122                         m4x4_t temp;
 123                         m4x4_identity( temp );
 124                         temp[5] = (vec_t) cx; temp[6] = (vec_t) sx;
 125                         temp[9] = (vec_t)-sx; temp[10] = (vec_t) cx;
 126                         m4x4_premultiply_by_m4x4( matrix, temp );
 127                         m4x4_identity( temp );
 128                         temp[0] = (vec_t) cy; temp[2] = (vec_t)-sy;
 129                         temp[8] = (vec_t) sy; temp[10] = (vec_t) cy;
 130                         m4x4_premultiply_by_m4x4( matrix, temp );
 131                 }
 132                 break;
 133
 134         case eXZY:
 135                 m4x4_identity( matrix );
 136                 matrix[5] = (vec_t) cx; matrix[6] = (vec_t) sx;
 137                 matrix[9] = (vec_t)-sx; matrix[10] = (vec_t) cx;
 138
 139                 {
 140                         m4x4_t temp;
 141                         m4x4_identity( temp );
 142                         temp[0] = (vec_t) cz; temp[1] = (vec_t) sz;
 143                         temp[4] = (vec_t)-sz; temp[5] = (vec_t) cz;
 144                         m4x4_premultiply_by_m4x4( matrix, temp );
 145                         m4x4_identity( temp );
 146                         temp[0] = (vec_t) cy; temp[2] = (vec_t)-sy;
 147                         temp[8] = (vec_t) sy; temp[10] = (vec_t) cy;
 148                         m4x4_premultiply_by_m4x4( matrix, temp );
 149                 }
 150                 break;
 151
 152         case eYXZ:
 153
 154 /* transposed
 155  |  cy.cz + sx.sy.-sz + -cx.sy.0   0.cz + cx.-sz + sx.0   sy.cz + -sx.cy.-sz + cx.cy.0 |
 156  |  cy.sz + sx.sy.cz + -cx.sy.0    0.sz + cx.cz + sx.0    sy.sz + -sx.cy.cz + cx.cy.0  |
 157  |  cy.0 + sx.sy.0 + -cx.sy.1      0.0 + cx.0 + sx.1      sy.0 + -sx.cy.0 + cx.cy.1    |
 158  */
 159
 160 #if 1
 161
 162                 {
 163                         matrix[0]  = (vec_t)( cy * cz + sx * sy * -sz );
 164                         matrix[1]  = (vec_t)( cy * sz + sx * sy * cz );
 165                         matrix[2]  = (vec_t)( -cx * sy );
 166                         matrix[4]  = (vec_t)( cx * -sz );
 167                         matrix[5]  = (vec_t)( cx * cz );
 168                         matrix[6]  = (vec_t)( sx );
 169                         matrix[8]  = (vec_t)( sy * cz + -sx * cy * -sz );
 170                         matrix[9]  = (vec_t)( sy * sz + -sx * cy * cz );
 171                         matrix[10] = (vec_t)( cx * cy );
 172                 }
 173
 174                 matrix[12]  =  matrix[13] = matrix[14] = matrix[3] = matrix[7] = matrix[11] = 0;
 175                 matrix[15] =  1;
 176
 177 #else
 178
 179                 m4x4_identity( matrix );
 180                 matrix[0] = (vec_t) cy; matrix[2] = (vec_t)-sy;
 181                 matrix[8] = (vec_t) sy; matrix[10] = (vec_t) cy;
 182
 183                 {
 184                         m4x4_t temp;
 185                         m4x4_identity( temp );
 186                         temp[5] = (vec_t) cx; temp[6] = (vec_t) sx;
 187                         temp[9] = (vec_t)-sx; temp[10] = (vec_t) cx;
 188                         m4x4_premultiply_by_m4x4( matrix, temp );
 189                         m4x4_identity( temp );
 190                         temp[0] = (vec_t) cz; temp[1] = (vec_t) sz;
 191                         temp[4] = (vec_t)-sz; temp[5] = (vec_t) cz;
 192                         m4x4_premultiply_by_m4x4( matrix, temp );
 193                 }
 194 #endif
 195                 break;
 196
 197         case eZYX:
 198 #if 1
 199
 200                 {
 201                         matrix[0]  = (vec_t)( cy * cz );
 202                         matrix[4]  = (vec_t)( cy * -sz );
 203                         matrix[8]  = (vec_t)sy;
 204                         matrix[1]  = (vec_t)( sx * sy * cz + cx * sz );
 205                         matrix[5]  = (vec_t)( sx * sy * -sz + cx * cz );
 206                         matrix[9]  = (vec_t)( -sx * cy );
 207                         matrix[2]  = (vec_t)( cx * -sy * cz + sx * sz );
 208                         matrix[6]  = (vec_t)( cx * -sy * -sz + sx * cz );
 209                         matrix[10] = (vec_t)( cx * cy );
 210                 }
 211
 212                 matrix[12]  =  matrix[13] = matrix[14] = matrix[3] = matrix[7] = matrix[11] = 0;
 213                 matrix[15] =  1;
 214
 215 #else
 216
 217                 m4x4_identity( matrix );
 218                 matrix[0] = (vec_t) cz; matrix[1] = (vec_t) sz;
 219                 matrix[4] = (vec_t)-sz; matrix[5] = (vec_t) cz;
 220                 {
 221                         m4x4_t temp;
 222                         m4x4_identity( temp );
 223                         temp[0] = (vec_t) cy; temp[2] = (vec_t)-sy;
 224                         temp[8] = (vec_t) sy; temp[10] = (vec_t) cy;
 225                         m4x4_premultiply_by_m4x4( matrix, temp );
 226                         m4x4_identity( temp );
 227                         temp[5] = (vec_t) cx; temp[6] = (vec_t) sx;
 228                         temp[9] = (vec_t)-sx; temp[10] = (vec_t) cx;
 229                         m4x4_premultiply_by_m4x4( matrix, temp );
 230                 }
 231
 232 #endif
 233                 break;
 234
 235         }
 236
 237 }
 238
 239 void m4x4_scale_for_vec3( m4x4_t matrix, const vec3_t scale ){
 240         matrix[1] = matrix[2] = matrix[3] =
 241                                                                 matrix[4] = matrix[6] = matrix[7] =
 242                                                                                                                         matrix[8] = matrix[9] = matrix[11] =
 243                                                                                                                                                                                 matrix[12] = matrix[13] = matrix[14] = 0;
 244
 245         matrix[15] = 1;
 246
 247         matrix[0] = scale[0];
 248         matrix[5] = scale[1];
 249         matrix[10] = scale[2];
 250 }
 251
 252 void m4x4_rotation_for_quat( m4x4_t matrix, const vec4_t rotation ){
 253         float xx,xy,xz,xw,yy,yz,yw,zz,zw;
 254
 255         xx      = rotation[0] * rotation[0];
 256         xy      = rotation[0] * rotation[1];
 257         xz      = rotation[0] * rotation[2];
 258         xw      = rotation[0] * rotation[3];
 259
 260         yy      = rotation[1] * rotation[1];
 261         yz      = rotation[1] * rotation[2];
 262         yw      = rotation[1] * rotation[3];
 263
 264         zz      = rotation[2] * rotation[2];
 265         zw      = rotation[2] * rotation[3];
 266
 267         matrix[0]  = 1 - 2 * ( yy + zz );
 268         matrix[4]  =     2 * ( xy - zw );
 269         matrix[8]  =     2 * ( xz + yw );
 270
 271         matrix[1]  =     2 * ( xy + zw );
 272         matrix[5]  = 1 - 2 * ( xx + zz );
 273         matrix[9]  =     2 * ( yz - xw );
 274
 275         matrix[2]  =     2 * ( xz - yw );
 276         matrix[6]  =     2 * ( yz + xw );
 277         matrix[10] = 1 - 2 * ( xx + yy );
 278
 279         matrix[3]  = matrix[7] = matrix[11] = matrix[12] = matrix[13] = matrix[14] = 0;
 280         matrix[15] = 1;
 281 }
 282
 283 void m4x4_rotation_for_axisangle( m4x4_t matrix, const vec3_t axis, vec_t angle ){
 284         vec4_t rotation;
 285         angle *= 0.5;
 286
 287         rotation[3] = (float)sin( (float)( angle ) );
 288
 289         rotation[0]    = axis[0] * rotation[3];
 290         rotation[1]    = axis[1] * rotation[3];
 291         rotation[2]    = axis[2] * rotation[3];
 292         rotation[3]    = (float)cos( (float)( angle ) );
 293
 294         m4x4_rotation_for_quat( matrix, rotation );
 295 }
 296
 297 void m4x4_translate_by_vec3( m4x4_t matrix, const vec3_t translation ){
 298         m4x4_t temp;
 299         m4x4_translation_for_vec3( temp, translation );
 300         m4x4_multiply_by_m4x4( matrix, temp );
 301 }
 302
 303 void m4x4_rotate_by_vec3( m4x4_t matrix, const vec3_t euler, eulerOrder_t order ){
 304         m4x4_t temp;
 305         m4x4_rotation_for_vec3( temp, euler, order );
 306         m4x4_multiply_by_m4x4( matrix, temp );
 307 }
 308
 309 void m4x4_scale_by_vec3( m4x4_t matrix, const vec3_t scale ){
 310         m4x4_t temp;
 311         m4x4_scale_for_vec3( temp, scale );
 312         m4x4_multiply_by_m4x4( matrix, temp );
 313 }
 314
 315 void m4x4_rotate_by_quat( m4x4_t matrix, const vec4_t rotation ){
 316         m4x4_t temp;
 317         m4x4_rotation_for_quat( temp, rotation );
 318         m4x4_multiply_by_m4x4( matrix, temp );
 319 }
 320
 321 void m4x4_rotate_by_axisangle( m4x4_t matrix, const vec3_t axis, vec_t angle ){
 322         m4x4_t temp;
 323         m4x4_rotation_for_axisangle( temp, axis, angle );
 324         m4x4_multiply_by_m4x4( matrix, temp );
 325 }
 326
 327 void m4x4_transform_by_vec3( m4x4_t matrix, const vec3_t translation, const vec3_t euler, eulerOrder_t order, const vec3_t scale ){
 328         m4x4_translate_by_vec3( matrix, translation );
 329         m4x4_rotate_by_vec3( matrix, euler, order );
 330         m4x4_scale_by_vec3( matrix, scale );
 331 }
 332
 333 void m4x4_pivoted_rotate_by_vec3( m4x4_t matrix, const vec3_t euler, eulerOrder_t order, const vec3_t pivotpoint ){
 334         vec3_t vec3_temp;
 335         VectorNegative( pivotpoint, vec3_temp );
 336
 337         m4x4_translate_by_vec3( matrix, pivotpoint );
 338         m4x4_rotate_by_vec3( matrix, euler, order );
 339         m4x4_translate_by_vec3( matrix, vec3_temp );
 340 }
 341
 342 void m4x4_pivoted_scale_by_vec3( m4x4_t matrix, const vec3_t scale, const vec3_t pivotpoint ){
 343         vec3_t vec3_temp;
 344         VectorNegative( pivotpoint, vec3_temp );
 345
 346         m4x4_translate_by_vec3( matrix, pivotpoint );
 347         m4x4_scale_by_vec3( matrix, scale );
 348         m4x4_translate_by_vec3( matrix, vec3_temp );
 349 }
 350
 351 void m4x4_pivoted_transform_by_vec3( m4x4_t matrix, const vec3_t translation, const vec3_t euler, eulerOrder_t order, const vec3_t scale, const vec3_t pivotpoint ){
 352         vec3_t vec3_temp;
 353
 354         VectorAdd( pivotpoint, translation, vec3_temp );
 355         m4x4_translate_by_vec3( matrix, vec3_temp );
 356         m4x4_rotate_by_vec3( matrix, euler, order );
 357         m4x4_scale_by_vec3( matrix, scale );
 358         VectorNegative( pivotpoint, vec3_temp );
 359         m4x4_translate_by_vec3( matrix, vec3_temp );
 360 }
 361
 362 void m4x4_pivoted_rotate_by_quat( m4x4_t matrix, const vec4_t rotation, const vec3_t pivotpoint ){
 363         vec3_t vec3_temp;
 364         VectorNegative( pivotpoint, vec3_temp );
 365
 366         m4x4_translate_by_vec3( matrix, pivotpoint );
 367         m4x4_rotate_by_quat( matrix, rotation );
 368         m4x4_translate_by_vec3( matrix, vec3_temp );
 369 }
 370
 371 void m4x4_pivoted_rotate_by_axisangle( m4x4_t matrix, const vec3_t axis, vec_t angle, const vec3_t pivotpoint ){
 372         vec3_t vec3_temp;
 373         VectorNegative( pivotpoint, vec3_temp );
 374
 375         m4x4_translate_by_vec3( matrix, pivotpoint );
 376         m4x4_rotate_by_axisangle( matrix, axis, angle );
 377         m4x4_translate_by_vec3( matrix, vec3_temp );
 378 }
 379
 380
 381 /*
 382    A = A.B
 383
 384    A0 = B0 * A0 + B1 * A4 + B2 * A8 + B3 * A12
 385    A4 = B4 * A0 + B5 * A4 + B6 * A8 + B7 * A12
 386    A8 = B8 * A0 + B9 * A4 + B10* A8 + B11* A12
 387    A12= B12* A0 + B13* A4 + B14* A8 + B15* A12
 388
 389    A1 = B0 * A1 + B1 * A5 + B2 * A9 + B3 * A13
 390    A5 = B4 * A1 + B5 * A5 + B6 * A9 + B7 * A13
 391    A9 = B8 * A1 + B9 * A5 + B10* A9 + B11* A13
 392    A13= B12* A1 + B13* A5 + B14* A9 + B15* A13
 393
 394    A2 = B0 * A2 + B1 * A6 + B2 * A10+ B3 * A14
 395    A6 = B4 * A2 + B5 * A6 + B6 * A10+ B7 * A14
 396    A10= B8 * A2 + B9 * A6 + B10* A10+ B11* A14
 397    A14= B12* A2 + B13* A6 + B14* A10+ B15* A14
 398
 399    A3 = B0 * A3 + B1 * A7 + B2 * A11+ B3 * A15
 400    A7 = B4 * A3 + B5 * A7 + B6 * A11+ B7 * A15
 401    A11= B8 * A3 + B9 * A7 + B10* A11+ B11* A15
 402    A15= B12* A3 + B13* A7 + B14* A11+ B15* A15
 403  */
 404
 405 void m4x4_multiply_by_m4x4( m4x4_t dst, const m4x4_t src ){
 406         vec_t dst0, dst1, dst2, dst3;
 407
 408 #if 1
 409
 410         dst0 = src[0] * dst[0] + src[1] * dst[4] + src[2] * dst[8] + src[3] * dst[12];
 411         dst1 = src[4] * dst[0] + src[5] * dst[4] + src[6] * dst[8] + src[7] * dst[12];
 412         dst2 = src[8] * dst[0] + src[9] * dst[4] + src[10] * dst[8] + src[11] * dst[12];
 413         dst3 = src[12] * dst[0] + src[13] * dst[4] + src[14] * dst[8] + src[15] * dst[12];
 414         dst[0] = dst0; dst[4] = dst1; dst[8] = dst2; dst[12] = dst3;
 415
 416         dst0 = src[0] * dst[1] + src[1] * dst[5] + src[2] * dst[9] + src[3] * dst[13];
 417         dst1 = src[4] * dst[1] + src[5] * dst[5] + src[6] * dst[9] + src[7] * dst[13];
 418         dst2 = src[8] * dst[1] + src[9] * dst[5] + src[10] * dst[9] + src[11] * dst[13];
 419         dst3 = src[12] * dst[1] + src[13] * dst[5] + src[14] * dst[9] + src[15] * dst[13];
 420         dst[1] = dst0; dst[5] = dst1; dst[9] = dst2; dst[13] = dst3;
 421
 422         dst0 = src[0] * dst[2] + src[1] * dst[6] + src[2] * dst[10] + src[3] * dst[14];
 423         dst1 = src[4] * dst[2] + src[5] * dst[6] + src[6] * dst[10] + src[7] * dst[14];
 424         dst2 = src[8] * dst[2] + src[9] * dst[6] + src[10] * dst[10] + src[11] * dst[14];
 425         dst3 = src[12] * dst[2] + src[13] * dst[6] + src[14] * dst[10] + src[15] * dst[14];
 426         dst[2] = dst0; dst[6] = dst1; dst[10] = dst2; dst[14] = dst3;
 427
 428         dst0 = src[0] * dst[3] + src[1] * dst[7] + src[2] * dst[11] + src[3] * dst[15];
 429         dst1 = src[4] * dst[3] + src[5] * dst[7] + src[6] * dst[11] + src[7] * dst[15];
 430         dst2 = src[8] * dst[3] + src[9] * dst[7] + src[10] * dst[11] + src[11] * dst[15];
 431         dst3 = src[12] * dst[3] + src[13] * dst[7] + src[14] * dst[11] + src[15] * dst[15];
 432         dst[3] = dst0; dst[7] = dst1; dst[11] = dst2; dst[15] = dst3;
 433
 434 #else
 435
 436         vec_t * p = dst;
 437         for ( int i = 0; i < 4; i++ )
 438         {
 439                 dst1 =  src[0]  * p[0];
 440                 dst1 += src[1]  * p[4];
 441                 dst1 += src[2]  * p[8];
 442                 dst1 += src[3]  * p[12];
 443                 dst2 =  src[4]  * p[0];
 444                 dst2 += src[5]  * p[4];
 445                 dst2 += src[6]  * p[8];
 446                 dst2 += src[7]  * p[12];
 447                 dst3 =  src[8]  * p[0];
 448                 dst3 += src[9]  * p[4];
 449                 dst3 += src[10] * p[8];
 450                 dst3 += src[11] * p[12];
 451                 dst4 =  src[12] * p[0];
 452                 dst4 += src[13] * p[4];
 453                 dst4 += src[14] * p[8];
 454                 dst4 += src[15] * p[12];
 455
 456                 p[0] = dst1;
 457                 p[4] = dst2;
 458                 p[8] = dst3;
 459                 p[12] = dst4;
 460                 p++;
 461         }
 462
 463 #endif
 464 }
 465
 466 /*
 467    A = B.A
 468
 469    A0 = A0 * B0 + A1 * B4 + A2 * B8 + A3 * B12
 470    A1 = A0 * B1 + A1 * B5 + A2 * B9 + A3 * B13
 471    A2 = A0 * B2 + A1 * B6 + A2 * B10+ A3 * B14
 472    A3 = A0 * B3 + A1 * B7 + A2 * B11+ A3 * B15
 473
 474    A4 = A4 * B0 + A5 * B4 + A6 * B8 + A7 * B12
 475    A5 = A4 * B1 + A5 * B5 + A6 * B9 + A7 * B13
 476    A6 = A4 * B2 + A5 * B6 + A6 * B10+ A7 * B14
 477    A7 = A4 * B3 + A5 * B7 + A6 * B11+ A7 * B15
 478
 479    A8 = A8 * B0 + A9 * B4 + A10* B8 + A11* B12
 480    A9 = A8 * B1 + A9 * B5 + A10* B9 + A11* B13
 481    A10= A8 * B2 + A9 * B6 + A10* B10+ A11* B14
 482    A11= A8 * B3 + A9 * B7 + A10* B11+ A11* B15
 483
 484    A12= A12* B0 + A13* B4 + A14* B8 + A15* B12
 485    A13= A12* B1 + A13* B5 + A14* B9 + A15* B13
 486    A14= A12* B2 + A13* B6 + A14* B10+ A15* B14
 487    A15= A12* B3 + A13* B7 + A14* B11+ A15* B15
 488  */
 489
 490 void m4x4_premultiply_by_m4x4( m4x4_t dst, const m4x4_t src ){
 491         vec_t dst0, dst1, dst2, dst3;
 492
 493 #if 1
 494
 495         dst0 = dst[0] * src[0] + dst[1] * src[4] + dst[2] * src[8] + dst[3] * src[12];
 496         dst1 = dst[0] * src[1] + dst[1] * src[5] + dst[2] * src[9] + dst[3] * src[13];
 497         dst2 = dst[0] * src[2] + dst[1] * src[6] + dst[2] * src[10] + dst[3] * src[14];
 498         dst3 = dst[0] * src[3] + dst[1] * src[7] + dst[2] * src[11] + dst[3] * src[15];
 499         dst[0] = dst0; dst[1] = dst1; dst[2] = dst2; dst[3] = dst3;
 500
 501         dst0 = dst[4] * src[0] + dst[5] * src[4] + dst[6] * src[8] + dst[7] * src[12];
 502         dst1 = dst[4] * src[1] + dst[5] * src[5] + dst[6] * src[9] + dst[7] * src[13];
 503         dst2 = dst[4] * src[2] + dst[5] * src[6] + dst[6] * src[10] + dst[7] * src[14];
 504         dst3 = dst[4] * src[3] + dst[5] * src[7] + dst[6] * src[11] + dst[7] * src[15];
 505         dst[4] = dst0; dst[5] = dst1; dst[6] = dst2; dst[7] = dst3;
 506
 507         dst0 = dst[8] * src[0] + dst[9] * src[4] + dst[10] * src[8] + dst[11] * src[12];
 508         dst1 = dst[8] * src[1] + dst[9] * src[5] + dst[10] * src[9] + dst[11] * src[13];
 509         dst2 = dst[8] * src[2] + dst[9] * src[6] + dst[10] * src[10] + dst[11] * src[14];
 510         dst3 = dst[8] * src[3] + dst[9] * src[7] + dst[10] * src[11] + dst[11] * src[15];
 511         dst[8] = dst0; dst[9] = dst1; dst[10] = dst2; dst[11] = dst3;
 512
 513         dst0 = dst[12] * src[0] + dst[13] * src[4] + dst[14] * src[8] + dst[15] * src[12];
 514         dst1 = dst[12] * src[1] + dst[13] * src[5] + dst[14] * src[9] + dst[15] * src[13];
 515         dst2 = dst[12] * src[2] + dst[13] * src[6] + dst[14] * src[10] + dst[15] * src[14];
 516         dst3 = dst[12] * src[3] + dst[13] * src[7] + dst[14] * src[11] + dst[15] * src[15];
 517         dst[12] = dst0; dst[13] = dst1; dst[14] = dst2; dst[15] = dst3;
 518
 519 #else
 520
 521         vec_t* p = dst;
 522         for ( int i = 0; i < 4; i++ )
 523         {
 524                 dst1 =  src[0]  * p[0];
 525                 dst2 =  src[1]  * p[0];
 526                 dst3 =  src[2]  * p[0];
 527                 dst4 =  src[3]  * p[0];
 528                 dst1 += src[4]  * p[1];
 529                 dst2 += src[5]  * p[1];
 530                 dst3 += src[6]  * p[1];
 531                 dst4 += src[7]  * p[1];
 532                 dst1 += src[8]  * p[2];
 533                 dst2 += src[9]  * p[2];
 534                 dst4 += src[11] * p[2];
 535                 dst3 += src[10] * p[2];
 536                 dst1 += src[12] * p[3];
 537                 dst2 += src[13] * p[3];
 538                 dst3 += src[14] * p[3];
 539                 dst4 += src[15] * p[3];
 540
 541                 *p++ = dst1;
 542                 *p++ = dst2;
 543                 *p++ = dst3;
 544                 *p++ = dst4;
 545         }
 546
 547 #endif
 548 }
 549
 550 void m4x4_transform_point( const m4x4_t matrix, vec3_t point ){
 551         float out1, out2, out3;
 552
 553         out1 =  matrix[0]  * point[0];
 554         out2 =  matrix[1]  * point[0];
 555         out3 =  matrix[2]  * point[0];
 556         out1 += matrix[4]  * point[1];
 557         out2 += matrix[5]  * point[1];
 558         out3 += matrix[6]  * point[1];
 559         out1 += matrix[8]  * point[2];
 560         out2 += matrix[9]  * point[2];
 561         out3 += matrix[10] * point[2];
 562         out1 += matrix[12];
 563         out2 += matrix[13];
 564         out3 += matrix[14];
 565
 566         point[0] = out1;
 567         point[1] = out2;
 568         point[2] = out3;
 569 }
 570
 571 void m4x4_transform_normal( const m4x4_t matrix, vec3_t normal ){
 572         float out1, out2, out3;
 573
 574         out1 =  matrix[0]  * normal[0];
 575         out2 =  matrix[1]  * normal[0];
 576         out3 =  matrix[2]  * normal[0];
 577         out1 += matrix[4]  * normal[1];
 578         out2 += matrix[5]  * normal[1];
 579         out3 += matrix[6]  * normal[1];
 580         out1 += matrix[8]  * normal[2];
 581         out2 += matrix[9]  * normal[2];
 582         out3 += matrix[10] * normal[2];
 583
 584         normal[0] = out1;
 585         normal[1] = out2;
 586         normal[2] = out3;
 587 }
 588
 589 void m4x4_transform_vec4( const m4x4_t matrix, vec4_t vector ){
 590         float out1, out2, out3, out4;
 591
 592         out1 =  matrix[0]  * vector[0];
 593         out2 =  matrix[1]  * vector[0];
 594         out3 =  matrix[2]  * vector[0];
 595         out4 =  matrix[3]  * vector[0];
 596         out1 += matrix[4]  * vector[1];
 597         out2 += matrix[5]  * vector[1];
 598         out3 += matrix[6]  * vector[1];
 599         out4 += matrix[7]  * vector[1];
 600         out1 += matrix[8]  * vector[2];
 601         out2 += matrix[9]  * vector[2];
 602         out3 += matrix[10] * vector[2];
 603         out4 += matrix[11] * vector[2];
 604         out1 += matrix[12] * vector[3];
 605         out2 += matrix[13] * vector[3];
 606         out3 += matrix[14] * vector[3];
 607         out4 += matrix[15] * vector[3];
 608
 609         vector[0] = out1;
 610         vector[1] = out2;
 611         vector[2] = out3;
 612         vector[3] = out4;
 613 }
 614
 615 void m4x4_transpose( m4x4_t matrix ){
 616         int i, j;
 617         float temp, *p1, *p2;
 618
 619         for ( i = 1; i < 4; i++ ) {
 620                 for ( j = 0; j < i; j++ ) {
 621                         p1 = matrix + ( j * 4 + i );
 622                         p2 = matrix + ( i * 4 + j );
 623                         temp = *p1;
 624                         *p1 = *p2;
 625                         *p2 = temp;
 626                 }
 627         }
 628 }
 629
 630 void m4x4_orthogonal_invert( m4x4_t matrix ){
 631         float temp;
 632
 633         temp = -matrix[3];
 634         matrix[3] = matrix[12];
 635         matrix[12] = temp;
 636
 637         temp = -matrix[7];
 638         matrix[7] = matrix[13];
 639         matrix[13] = temp;
 640
 641         temp = -matrix[11];
 642         matrix[11] = matrix[14];
 643         matrix[14] = temp;
 644
 645         /*
 646            temp = matrix[1];
 647            matrix[1] = matrix[4];
 648            matrix[4] = temp;
 649
 650            temp = matrix[2];
 651            matrix[2] = matrix[8];
 652            matrix[8] = temp;
 653
 654            temp = matrix[6];
 655            matrix[6] = matrix[9];
 656            matrix[9] = temp;
 657
 658            matrix[3] = -matrix[3];
 659            matrix[7] = -matrix[7];
 660            matrix[11] = -matrix[11];
 661          */
 662 }
 663
 664 float m3_det( m3x3_t mat ){
 665         float det;
 666
 667         det = mat[0] * ( mat[4] * mat[8] - mat[7] * mat[5] )
 668                   - mat[1] * ( mat[3] * mat[8] - mat[6] * mat[5] )
 669                   + mat[2] * ( mat[3] * mat[7] - mat[6] * mat[4] );
 670
 671         return( det );
 672 }
 673
 674 /*
 675    void m3_inverse( m3x3_t mr, m3x3_t ma )
 676    {
 677    float det = m3_det( ma );
 678
 679    if ( fabs( det ) < 0.0005 )
 680    {
 681     m3_identity( ma );
 682     return;
 683    }
 684
 685    mr[0] =    ma[4]*ma[8] - ma[5]*ma[7]   / det;
 686    mr[1] = -( ma[1]*ma[8] - ma[7]*ma[2] ) / det;
 687    mr[2] =    ma[1]*ma[5] - ma[4]*ma[2]   / det;
 688
 689    mr[3] = -( ma[3]*ma[8] - ma[5]*ma[6] ) / det;
 690    mr[4] =    ma[0]*ma[8] - ma[6]*ma[2]   / det;
 691    mr[5] = -( ma[0]*ma[5] - ma[3]*ma[2] ) / det;
 692
 693    mr[6] =    ma[3]*ma[7] - ma[6]*ma[4]   / det;
 694    mr[7] = -( ma[0]*ma[7] - ma[6]*ma[1] ) / det;
 695    mr[8] =    ma[0]*ma[4] - ma[1]*ma[3]   / det;
 696    }
 697  */
 698
 699 void m4_submat( m4x4_t mr, m3x3_t mb, int i, int j ){
 700         int ti, tj, idst, jdst;
 701
 702         idst = 0;
 703         for ( ti = 0; ti < 4; ti++ )
 704         {
 705                 if ( ti < i ) {
 706                         idst = ti;
 707                 }
 708                 else
 709                 if ( ti > i ) {
 710                         idst = ti - 1;
 711                 }
 712
 713                 for ( tj = 0; tj < 4; tj++ )
 714                 {
 715                         if ( tj < j ) {
 716                                 jdst = tj;
 717                         }
 718                         else
 719                         if ( tj > j ) {
 720                                 jdst = tj - 1;
 721                         }
 722
 723                         if ( ti != i && tj != j ) {
 724                                 mb[idst * 3 + jdst] = mr[ti * 4 + tj ];
 725                         }
 726                 }
 727         }
 728 }
 729
 730 float m4_det( m4x4_t mr ){
 731         float det, result = 0, i = 1;
 732         m3x3_t msub3;
 733         int n;
 734
 735         for ( n = 0; n < 4; n++, i *= -1 )
 736         {
 737                 m4_submat( mr, msub3, 0, n );
 738
 739                 det     = m3_det( msub3 );
 740                 result += mr[n] * det * i;
 741         }
 742
 743         return result;
 744 }
 745
 746 int m4x4_invert( m4x4_t matrix ){
 747         float mdet = m4_det( matrix );
 748         m3x3_t mtemp;
 749         int i, j, sign;
 750         m4x4_t m4x4_temp;
 751
 752         if ( fabs( mdet ) < 0.0000000001 ) { //% 0.0005
 753                 return 1;
 754         }
 755
 756         memcpy( m4x4_temp, matrix, sizeof( m4x4_t ) );
 757
 758         for ( i = 0; i < 4; i++ )
 759                 for ( j = 0; j < 4; j++ )
 760                 {
 761                         sign = 1 - ( ( i + j ) % 2 ) * 2;
 762
 763                         m4_submat( m4x4_temp, mtemp, i, j );
 764
 765                         matrix[i + j * 4] = ( m3_det( mtemp ) * sign ) / mdet;
 766                 }
 767
 768         return 0;
 769 }