libs/mathlib/m4x4.c

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