matrixlib.c

   1
   2 #include <math.h>
   3 #include "matrixlib.h"
   4
   5 const matrix4x4_t identitymatrix =
   6 {
   7         {
   8                 {1, 0, 0, 0},
   9                 {0, 1, 0, 0},
  10                 {0, 0, 1, 0},
  11                 {0, 0, 0, 1}
  12         }
  13 };
  14
  15 void Matrix4x4_Copy (matrix4x4_t *out, const matrix4x4_t *in)
  16 {
  17         *out = *in;
  18 }
  19
  20 void Matrix4x4_CopyRotateOnly (matrix4x4_t *out, const matrix4x4_t *in)
  21 {
  22         out->m[0][0] = in->m[0][0];
  23         out->m[0][1] = in->m[0][1];
  24         out->m[0][2] = in->m[0][2];
  25         out->m[0][3] = 0.0f;
  26         out->m[1][0] = in->m[1][0];
  27         out->m[1][1] = in->m[1][1];
  28         out->m[1][2] = in->m[1][2];
  29         out->m[1][3] = 0.0f;
  30         out->m[2][0] = in->m[2][0];
  31         out->m[2][1] = in->m[2][1];
  32         out->m[2][2] = in->m[2][2];
  33         out->m[2][3] = 0.0f;
  34         out->m[3][0] = 0.0f;
  35         out->m[3][1] = 0.0f;
  36         out->m[3][2] = 0.0f;
  37         out->m[3][3] = 1.0f;
  38 }
  39
  40 void Matrix4x4_CopyTranslateOnly (matrix4x4_t *out, const matrix4x4_t *in)
  41 {
  42 #ifdef MATRIX4x4_OPENGLORIENTATION
  43         out->m[0][0] = 1.0f;
  44         out->m[1][0] = 0.0f;
  45         out->m[2][0] = 0.0f;
  46         out->m[3][0] = in->m[0][3];
  47         out->m[0][1] = 0.0f;
  48         out->m[1][1] = 1.0f;
  49         out->m[2][1] = 0.0f;
  50         out->m[3][1] = in->m[1][3];
  51         out->m[0][2] = 0.0f;
  52         out->m[1][2] = 0.0f;
  53         out->m[2][2] = 1.0f;
  54         out->m[3][2] = in->m[2][3];
  55         out->m[0][3] = 0.0f;
  56         out->m[1][3] = 0.0f;
  57         out->m[2][3] = 0.0f;
  58         out->m[3][3] = 1.0f;
  59 #else
  60         out->m[0][0] = 1.0f;
  61         out->m[0][1] = 0.0f;
  62         out->m[0][2] = 0.0f;
  63         out->m[0][3] = in->m[0][3];
  64         out->m[1][0] = 0.0f;
  65         out->m[1][1] = 1.0f;
  66         out->m[1][2] = 0.0f;
  67         out->m[1][3] = in->m[1][3];
  68         out->m[2][0] = 0.0f;
  69         out->m[2][1] = 0.0f;
  70         out->m[2][2] = 1.0f;
  71         out->m[2][3] = in->m[2][3];
  72         out->m[3][0] = 0.0f;
  73         out->m[3][1] = 0.0f;
  74         out->m[3][2] = 0.0f;
  75         out->m[3][3] = 1.0f;
  76 #endif
  77 }
  78
  79 void Matrix4x4_Concat (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2)
  80 {
  81 #ifdef MATRIX4x4_OPENGLORIENTATION
  82         out->m[0][0] = in1->m[0][0] * in2->m[0][0] + in1->m[1][0] * in2->m[0][1] + in1->m[2][0] * in2->m[0][2] + in1->m[3][0] * in2->m[0][3];
  83         out->m[1][0] = in1->m[0][0] * in2->m[1][0] + in1->m[1][0] * in2->m[1][1] + in1->m[2][0] * in2->m[1][2] + in1->m[3][0] * in2->m[1][3];
  84         out->m[2][0] = in1->m[0][0] * in2->m[2][0] + in1->m[1][0] * in2->m[2][1] + in1->m[2][0] * in2->m[2][2] + in1->m[3][0] * in2->m[2][3];
  85         out->m[3][0] = in1->m[0][0] * in2->m[3][0] + in1->m[1][0] * in2->m[3][1] + in1->m[2][0] * in2->m[3][2] + in1->m[3][0] * in2->m[3][3];
  86         out->m[0][1] = in1->m[0][1] * in2->m[0][0] + in1->m[1][1] * in2->m[0][1] + in1->m[2][1] * in2->m[0][2] + in1->m[3][1] * in2->m[0][3];
  87         out->m[1][1] = in1->m[0][1] * in2->m[1][0] + in1->m[1][1] * in2->m[1][1] + in1->m[2][1] * in2->m[1][2] + in1->m[3][1] * in2->m[1][3];
  88         out->m[2][1] = in1->m[0][1] * in2->m[2][0] + in1->m[1][1] * in2->m[2][1] + in1->m[2][1] * in2->m[2][2] + in1->m[3][1] * in2->m[2][3];
  89         out->m[3][1] = in1->m[0][1] * in2->m[3][0] + in1->m[1][1] * in2->m[3][1] + in1->m[2][1] * in2->m[3][2] + in1->m[3][1] * in2->m[3][3];
  90         out->m[0][2] = in1->m[0][2] * in2->m[0][0] + in1->m[1][2] * in2->m[0][1] + in1->m[2][2] * in2->m[0][2] + in1->m[3][2] * in2->m[0][3];
  91         out->m[1][2] = in1->m[0][2] * in2->m[1][0] + in1->m[1][2] * in2->m[1][1] + in1->m[2][2] * in2->m[1][2] + in1->m[3][2] * in2->m[1][3];
  92         out->m[2][2] = in1->m[0][2] * in2->m[2][0] + in1->m[1][2] * in2->m[2][1] + in1->m[2][2] * in2->m[2][2] + in1->m[3][2] * in2->m[2][3];
  93         out->m[3][2] = in1->m[0][2] * in2->m[3][0] + in1->m[1][2] * in2->m[3][1] + in1->m[2][2] * in2->m[3][2] + in1->m[3][2] * in2->m[3][3];
  94         out->m[0][3] = in1->m[0][3] * in2->m[0][0] + in1->m[1][3] * in2->m[0][1] + in1->m[2][3] * in2->m[0][2] + in1->m[3][3] * in2->m[0][3];
  95         out->m[1][3] = in1->m[0][3] * in2->m[1][0] + in1->m[1][3] * in2->m[1][1] + in1->m[2][3] * in2->m[1][2] + in1->m[3][3] * in2->m[1][3];
  96         out->m[2][3] = in1->m[0][3] * in2->m[2][0] + in1->m[1][3] * in2->m[2][1] + in1->m[2][3] * in2->m[2][2] + in1->m[3][3] * in2->m[2][3];
  97         out->m[3][3] = in1->m[0][3] * in2->m[3][0] + in1->m[1][3] * in2->m[3][1] + in1->m[2][3] * in2->m[3][2] + in1->m[3][3] * in2->m[3][3];
  98 #else
  99         out->m[0][0] = in1->m[0][0] * in2->m[0][0] + in1->m[0][1] * in2->m[1][0] + in1->m[0][2] * in2->m[2][0] + in1->m[0][3] * in2->m[3][0];
 100         out->m[0][1] = in1->m[0][0] * in2->m[0][1] + in1->m[0][1] * in2->m[1][1] + in1->m[0][2] * in2->m[2][1] + in1->m[0][3] * in2->m[3][1];
 101         out->m[0][2] = in1->m[0][0] * in2->m[0][2] + in1->m[0][1] * in2->m[1][2] + in1->m[0][2] * in2->m[2][2] + in1->m[0][3] * in2->m[3][2];
 102         out->m[0][3] = in1->m[0][0] * in2->m[0][3] + in1->m[0][1] * in2->m[1][3] + in1->m[0][2] * in2->m[2][3] + in1->m[0][3] * in2->m[3][3];
 103         out->m[1][0] = in1->m[1][0] * in2->m[0][0] + in1->m[1][1] * in2->m[1][0] + in1->m[1][2] * in2->m[2][0] + in1->m[1][3] * in2->m[3][0];
 104         out->m[1][1] = in1->m[1][0] * in2->m[0][1] + in1->m[1][1] * in2->m[1][1] + in1->m[1][2] * in2->m[2][1] + in1->m[1][3] * in2->m[3][1];
 105         out->m[1][2] = in1->m[1][0] * in2->m[0][2] + in1->m[1][1] * in2->m[1][2] + in1->m[1][2] * in2->m[2][2] + in1->m[1][3] * in2->m[3][2];
 106         out->m[1][3] = in1->m[1][0] * in2->m[0][3] + in1->m[1][1] * in2->m[1][3] + in1->m[1][2] * in2->m[2][3] + in1->m[1][3] * in2->m[3][3];
 107         out->m[2][0] = in1->m[2][0] * in2->m[0][0] + in1->m[2][1] * in2->m[1][0] + in1->m[2][2] * in2->m[2][0] + in1->m[2][3] * in2->m[3][0];
 108         out->m[2][1] = in1->m[2][0] * in2->m[0][1] + in1->m[2][1] * in2->m[1][1] + in1->m[2][2] * in2->m[2][1] + in1->m[2][3] * in2->m[3][1];
 109         out->m[2][2] = in1->m[2][0] * in2->m[0][2] + in1->m[2][1] * in2->m[1][2] + in1->m[2][2] * in2->m[2][2] + in1->m[2][3] * in2->m[3][2];
 110         out->m[2][3] = in1->m[2][0] * in2->m[0][3] + in1->m[2][1] * in2->m[1][3] + in1->m[2][2] * in2->m[2][3] + in1->m[2][3] * in2->m[3][3];
 111         out->m[3][0] = in1->m[3][0] * in2->m[0][0] + in1->m[3][1] * in2->m[1][0] + in1->m[3][2] * in2->m[2][0] + in1->m[3][3] * in2->m[3][0];
 112         out->m[3][1] = in1->m[3][0] * in2->m[0][1] + in1->m[3][1] * in2->m[1][1] + in1->m[3][2] * in2->m[2][1] + in1->m[3][3] * in2->m[3][1];
 113         out->m[3][2] = in1->m[3][0] * in2->m[0][2] + in1->m[3][1] * in2->m[1][2] + in1->m[3][2] * in2->m[2][2] + in1->m[3][3] * in2->m[3][2];
 114         out->m[3][3] = in1->m[3][0] * in2->m[0][3] + in1->m[3][1] * in2->m[1][3] + in1->m[3][2] * in2->m[2][3] + in1->m[3][3] * in2->m[3][3];
 115 #endif
 116 }
 117
 118 void Matrix4x4_Transpose (matrix4x4_t *out, const matrix4x4_t *in1)
 119 {
 120         out->m[0][0] = in1->m[0][0];
 121         out->m[0][1] = in1->m[1][0];
 122         out->m[0][2] = in1->m[2][0];
 123         out->m[0][3] = in1->m[3][0];
 124         out->m[1][0] = in1->m[0][1];
 125         out->m[1][1] = in1->m[1][1];
 126         out->m[1][2] = in1->m[2][1];
 127         out->m[1][3] = in1->m[3][1];
 128         out->m[2][0] = in1->m[0][2];
 129         out->m[2][1] = in1->m[1][2];
 130         out->m[2][2] = in1->m[2][2];
 131         out->m[2][3] = in1->m[3][2];
 132         out->m[3][0] = in1->m[0][3];
 133         out->m[3][1] = in1->m[1][3];
 134         out->m[3][2] = in1->m[2][3];
 135         out->m[3][3] = in1->m[3][3];
 136 }
 137
 138 void Matrix4x4_Invert_Simple (matrix4x4_t *out, const matrix4x4_t *in1)
 139 {
 140         // we only support uniform scaling, so assume the first row is enough
 141         // (note the lack of sqrt here, because we're trying to undo the scaling,
 142         // this means multiplying by the inverse scale twice - squaring it, which
 143         // makes the sqrt a waste of time)
 144 #if 1
 145         double scale = 1.0 / (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]);
 146 #else
 147         double scale = 3.0 / sqrt
 148                  (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]
 149                 + in1->m[1][0] * in1->m[1][0] + in1->m[1][1] * in1->m[1][1] + in1->m[1][2] * in1->m[1][2]
 150                 + in1->m[2][0] * in1->m[2][0] + in1->m[2][1] * in1->m[2][1] + in1->m[2][2] * in1->m[2][2]);
 151         scale *= scale;
 152 #endif
 153
 154         // invert the rotation by transposing and multiplying by the squared
 155         // recipricol of the input matrix scale as described above
 156         out->m[0][0] = in1->m[0][0] * scale;
 157         out->m[0][1] = in1->m[1][0] * scale;
 158         out->m[0][2] = in1->m[2][0] * scale;
 159         out->m[1][0] = in1->m[0][1] * scale;
 160         out->m[1][1] = in1->m[1][1] * scale;
 161         out->m[1][2] = in1->m[2][1] * scale;
 162         out->m[2][0] = in1->m[0][2] * scale;
 163         out->m[2][1] = in1->m[1][2] * scale;
 164         out->m[2][2] = in1->m[2][2] * scale;
 165
 166 #ifdef MATRIX4x4_OPENGLORIENTATION
 167         // invert the translate
 168         out->m[3][0] = -(in1->m[3][0] * out->m[0][0] + in1->m[3][1] * out->m[1][0] + in1->m[3][2] * out->m[2][0]);
 169         out->m[3][1] = -(in1->m[3][0] * out->m[0][1] + in1->m[3][1] * out->m[1][1] + in1->m[3][2] * out->m[2][1]);
 170         out->m[3][2] = -(in1->m[3][0] * out->m[0][2] + in1->m[3][1] * out->m[1][2] + in1->m[3][2] * out->m[2][2]);
 171
 172         // don't know if there's anything worth doing here
 173         out->m[0][3] = 0;
 174         out->m[1][3] = 0;
 175         out->m[2][3] = 0;
 176         out->m[3][3] = 1;
 177 #else
 178         // invert the translate
 179         out->m[0][3] = -(in1->m[0][3] * out->m[0][0] + in1->m[1][3] * out->m[0][1] + in1->m[2][3] * out->m[0][2]);
 180         out->m[1][3] = -(in1->m[0][3] * out->m[1][0] + in1->m[1][3] * out->m[1][1] + in1->m[2][3] * out->m[1][2]);
 181         out->m[2][3] = -(in1->m[0][3] * out->m[2][0] + in1->m[1][3] * out->m[2][1] + in1->m[2][3] * out->m[2][2]);
 182
 183         // don't know if there's anything worth doing here
 184         out->m[3][0] = 0;
 185         out->m[3][1] = 0;
 186         out->m[3][2] = 0;
 187         out->m[3][3] = 1;
 188 #endif
 189 }
 190
 191 void Matrix4x4_Normalize (matrix4x4_t *out, matrix4x4_t *in1)
 192 {
 193         // scale rotation matrix vectors to a length of 1
 194         // note: this is only designed to undo uniform scaling
 195         double scale = 1.0 / sqrt(in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]);
 196         *out = *in1;
 197         Matrix4x4_Scale(out, scale, 1);
 198 }
 199
 200 void Matrix4x4_CreateIdentity (matrix4x4_t *out)
 201 {
 202         out->m[0][0]=1.0f;
 203         out->m[0][1]=0.0f;
 204         out->m[0][2]=0.0f;
 205         out->m[0][3]=0.0f;
 206         out->m[1][0]=0.0f;
 207         out->m[1][1]=1.0f;
 208         out->m[1][2]=0.0f;
 209         out->m[1][3]=0.0f;
 210         out->m[2][0]=0.0f;
 211         out->m[2][1]=0.0f;
 212         out->m[2][2]=1.0f;
 213         out->m[2][3]=0.0f;
 214         out->m[3][0]=0.0f;
 215         out->m[3][1]=0.0f;
 216         out->m[3][2]=0.0f;
 217         out->m[3][3]=1.0f;
 218 }
 219
 220 void Matrix4x4_CreateTranslate (matrix4x4_t *out, double x, double y, double z)
 221 {
 222 #ifdef MATRIX4x4_OPENGLORIENTATION
 223         out->m[0][0]=1.0f;
 224         out->m[1][0]=0.0f;
 225         out->m[2][0]=0.0f;
 226         out->m[3][0]=x;
 227         out->m[0][1]=0.0f;
 228         out->m[1][1]=1.0f;
 229         out->m[2][1]=0.0f;
 230         out->m[3][1]=y;
 231         out->m[0][2]=0.0f;
 232         out->m[1][2]=0.0f;
 233         out->m[2][2]=1.0f;
 234         out->m[3][2]=z;
 235         out->m[0][3]=0.0f;
 236         out->m[1][3]=0.0f;
 237         out->m[2][3]=0.0f;
 238         out->m[3][3]=1.0f;
 239 #else
 240         out->m[0][0]=1.0f;
 241         out->m[0][1]=0.0f;
 242         out->m[0][2]=0.0f;
 243         out->m[0][3]=x;
 244         out->m[1][0]=0.0f;
 245         out->m[1][1]=1.0f;
 246         out->m[1][2]=0.0f;
 247         out->m[1][3]=y;
 248         out->m[2][0]=0.0f;
 249         out->m[2][1]=0.0f;
 250         out->m[2][2]=1.0f;
 251         out->m[2][3]=z;
 252         out->m[3][0]=0.0f;
 253         out->m[3][1]=0.0f;
 254         out->m[3][2]=0.0f;
 255         out->m[3][3]=1.0f;
 256 #endif
 257 }
 258
 259 void Matrix4x4_CreateRotate (matrix4x4_t *out, double angle, double x, double y, double z)
 260 {
 261         double len, c, s;
 262
 263         len = x*x+y*y+z*z;
 264         if (len != 0.0f)
 265                 len = 1.0f / sqrt(len);
 266         x *= len;
 267         y *= len;
 268         z *= len;
 269
 270         angle *= (-M_PI / 180.0);
 271         c = cos(angle);
 272         s = sin(angle);
 273
 274 #ifdef MATRIX4x4_OPENGLORIENTATION
 275         out->m[0][0]=x * x + c * (1 - x * x);
 276         out->m[1][0]=x * y * (1 - c) + z * s;
 277         out->m[2][0]=z * x * (1 - c) - y * s;
 278         out->m[3][0]=0.0f;
 279         out->m[0][1]=x * y * (1 - c) - z * s;
 280         out->m[1][1]=y * y + c * (1 - y * y);
 281         out->m[2][1]=y * z * (1 - c) + x * s;
 282         out->m[3][1]=0.0f;
 283         out->m[0][2]=z * x * (1 - c) + y * s;
 284         out->m[1][2]=y * z * (1 - c) - x * s;
 285         out->m[2][2]=z * z + c * (1 - z * z);
 286         out->m[3][2]=0.0f;
 287         out->m[0][3]=0.0f;
 288         out->m[1][3]=0.0f;
 289         out->m[2][3]=0.0f;
 290         out->m[3][3]=1.0f;
 291 #else
 292         out->m[0][0]=x * x + c * (1 - x * x);
 293         out->m[0][1]=x * y * (1 - c) + z * s;
 294         out->m[0][2]=z * x * (1 - c) - y * s;
 295         out->m[0][3]=0.0f;
 296         out->m[1][0]=x * y * (1 - c) - z * s;
 297         out->m[1][1]=y * y + c * (1 - y * y);
 298         out->m[1][2]=y * z * (1 - c) + x * s;
 299         out->m[1][3]=0.0f;
 300         out->m[2][0]=z * x * (1 - c) + y * s;
 301         out->m[2][1]=y * z * (1 - c) - x * s;
 302         out->m[2][2]=z * z + c * (1 - z * z);
 303         out->m[2][3]=0.0f;
 304         out->m[3][0]=0.0f;
 305         out->m[3][1]=0.0f;
 306         out->m[3][2]=0.0f;
 307         out->m[3][3]=1.0f;
 308 #endif
 309 }
 310
 311 void Matrix4x4_CreateScale (matrix4x4_t *out, double x)
 312 {
 313         out->m[0][0]=x;
 314         out->m[0][1]=0.0f;
 315         out->m[0][2]=0.0f;
 316         out->m[0][3]=0.0f;
 317         out->m[1][0]=0.0f;
 318         out->m[1][1]=x;
 319         out->m[1][2]=0.0f;
 320         out->m[1][3]=0.0f;
 321         out->m[2][0]=0.0f;
 322         out->m[2][1]=0.0f;
 323         out->m[2][2]=x;
 324         out->m[2][3]=0.0f;
 325         out->m[3][0]=0.0f;
 326         out->m[3][1]=0.0f;
 327         out->m[3][2]=0.0f;
 328         out->m[3][3]=1.0f;
 329 }
 330
 331 void Matrix4x4_CreateScale3 (matrix4x4_t *out, double x, double y, double z)
 332 {
 333         out->m[0][0]=x;
 334         out->m[0][1]=0.0f;
 335         out->m[0][2]=0.0f;
 336         out->m[0][3]=0.0f;
 337         out->m[1][0]=0.0f;
 338         out->m[1][1]=y;
 339         out->m[1][2]=0.0f;
 340         out->m[1][3]=0.0f;
 341         out->m[2][0]=0.0f;
 342         out->m[2][1]=0.0f;
 343         out->m[2][2]=z;
 344         out->m[2][3]=0.0f;
 345         out->m[3][0]=0.0f;
 346         out->m[3][1]=0.0f;
 347         out->m[3][2]=0.0f;
 348         out->m[3][3]=1.0f;
 349 }
 350
 351 void Matrix4x4_CreateFromQuakeEntity(matrix4x4_t *out, double x, double y, double z, double pitch, double yaw, double roll, double scale)
 352 {
 353         double angle, sr, sp, sy, cr, cp, cy;
 354
 355         if (roll)
 356         {
 357                 angle = yaw * (M_PI*2 / 360);
 358                 sy = sin(angle);
 359                 cy = cos(angle);
 360                 angle = pitch * (M_PI*2 / 360);
 361                 sp = sin(angle);
 362                 cp = cos(angle);
 363                 angle = roll * (M_PI*2 / 360);
 364                 sr = sin(angle);
 365                 cr = cos(angle);
 366 #ifdef MATRIX4x4_OPENGLORIENTATION
 367                 out->m[0][0] = (cp*cy) * scale;
 368                 out->m[1][0] = (sr*sp*cy+cr*-sy) * scale;
 369                 out->m[2][0] = (cr*sp*cy+-sr*-sy) * scale;
 370                 out->m[3][0] = x;
 371                 out->m[0][1] = (cp*sy) * scale;
 372                 out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
 373                 out->m[2][1] = (cr*sp*sy+-sr*cy) * scale;
 374                 out->m[3][1] = y;
 375                 out->m[0][2] = (-sp) * scale;
 376                 out->m[1][2] = (sr*cp) * scale;
 377                 out->m[2][2] = (cr*cp) * scale;
 378                 out->m[3][2] = z;
 379                 out->m[0][3] = 0;
 380                 out->m[1][3] = 0;
 381                 out->m[2][3] = 0;
 382                 out->m[3][3] = 1;
 383 #else
 384                 out->m[0][0] = (cp*cy) * scale;
 385                 out->m[0][1] = (sr*sp*cy+cr*-sy) * scale;
 386                 out->m[0][2] = (cr*sp*cy+-sr*-sy) * scale;
 387                 out->m[0][3] = x;
 388                 out->m[1][0] = (cp*sy) * scale;
 389                 out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
 390                 out->m[1][2] = (cr*sp*sy+-sr*cy) * scale;
 391                 out->m[1][3] = y;
 392                 out->m[2][0] = (-sp) * scale;
 393                 out->m[2][1] = (sr*cp) * scale;
 394                 out->m[2][2] = (cr*cp) * scale;
 395                 out->m[2][3] = z;
 396                 out->m[3][0] = 0;
 397                 out->m[3][1] = 0;
 398                 out->m[3][2] = 0;
 399                 out->m[3][3] = 1;
 400 #endif
 401         }
 402         else if (pitch)
 403         {
 404                 angle = yaw * (M_PI*2 / 360);
 405                 sy = sin(angle);
 406                 cy = cos(angle);
 407                 angle = pitch * (M_PI*2 / 360);
 408                 sp = sin(angle);
 409                 cp = cos(angle);
 410 #ifdef MATRIX4x4_OPENGLORIENTATION
 411                 out->m[0][0] = (cp*cy) * scale;
 412                 out->m[1][0] = (-sy) * scale;
 413                 out->m[2][0] = (sp*cy) * scale;
 414                 out->m[3][0] = x;
 415                 out->m[0][1] = (cp*sy) * scale;
 416                 out->m[1][1] = (cy) * scale;
 417                 out->m[2][1] = (sp*sy) * scale;
 418                 out->m[3][1] = y;
 419                 out->m[0][2] = (-sp) * scale;
 420                 out->m[1][2] = 0;
 421                 out->m[2][2] = (cp) * scale;
 422                 out->m[3][2] = z;
 423                 out->m[0][3] = 0;
 424                 out->m[1][3] = 0;
 425                 out->m[2][3] = 0;
 426                 out->m[3][3] = 1;
 427 #else
 428                 out->m[0][0] = (cp*cy) * scale;
 429                 out->m[0][1] = (-sy) * scale;
 430                 out->m[0][2] = (sp*cy) * scale;
 431                 out->m[0][3] = x;
 432                 out->m[1][0] = (cp*sy) * scale;
 433                 out->m[1][1] = (cy) * scale;
 434                 out->m[1][2] = (sp*sy) * scale;
 435                 out->m[1][3] = y;
 436                 out->m[2][0] = (-sp) * scale;
 437                 out->m[2][1] = 0;
 438                 out->m[2][2] = (cp) * scale;
 439                 out->m[2][3] = z;
 440                 out->m[3][0] = 0;
 441                 out->m[3][1] = 0;
 442                 out->m[3][2] = 0;
 443                 out->m[3][3] = 1;
 444 #endif
 445         }
 446         else if (yaw)
 447         {
 448                 angle = yaw * (M_PI*2 / 360);
 449                 sy = sin(angle);
 450                 cy = cos(angle);
 451 #ifdef MATRIX4x4_OPENGLORIENTATION
 452                 out->m[0][0] = (cy) * scale;
 453                 out->m[1][0] = (-sy) * scale;
 454                 out->m[2][0] = 0;
 455                 out->m[3][0] = x;
 456                 out->m[0][1] = (sy) * scale;
 457                 out->m[1][1] = (cy) * scale;
 458                 out->m[2][1] = 0;
 459                 out->m[3][1] = y;
 460                 out->m[0][2] = 0;
 461                 out->m[1][2] = 0;
 462                 out->m[2][2] = scale;
 463                 out->m[3][2] = z;
 464                 out->m[0][3] = 0;
 465                 out->m[1][3] = 0;
 466                 out->m[2][3] = 0;
 467                 out->m[3][3] = 1;
 468 #else
 469                 out->m[0][0] = (cy) * scale;
 470                 out->m[0][1] = (-sy) * scale;
 471                 out->m[0][2] = 0;
 472                 out->m[0][3] = x;
 473                 out->m[1][0] = (sy) * scale;
 474                 out->m[1][1] = (cy) * scale;
 475                 out->m[1][2] = 0;
 476                 out->m[1][3] = y;
 477                 out->m[2][0] = 0;
 478                 out->m[2][1] = 0;
 479                 out->m[2][2] = scale;
 480                 out->m[2][3] = z;
 481                 out->m[3][0] = 0;
 482                 out->m[3][1] = 0;
 483                 out->m[3][2] = 0;
 484                 out->m[3][3] = 1;
 485 #endif
 486         }
 487         else
 488         {
 489 #ifdef MATRIX4x4_OPENGLORIENTATION
 490                 out->m[0][0] = scale;
 491                 out->m[1][0] = 0;
 492                 out->m[2][0] = 0;
 493                 out->m[3][0] = x;
 494                 out->m[0][1] = 0;
 495                 out->m[1][1] = scale;
 496                 out->m[2][1] = 0;
 497                 out->m[3][1] = y;
 498                 out->m[0][2] = 0;
 499                 out->m[1][2] = 0;
 500                 out->m[2][2] = scale;
 501                 out->m[3][2] = z;
 502                 out->m[0][3] = 0;
 503                 out->m[1][3] = 0;
 504                 out->m[2][3] = 0;
 505                 out->m[3][3] = 1;
 506 #else
 507                 out->m[0][0] = scale;
 508                 out->m[0][1] = 0;
 509                 out->m[0][2] = 0;
 510                 out->m[0][3] = x;
 511                 out->m[1][0] = 0;
 512                 out->m[1][1] = scale;
 513                 out->m[1][2] = 0;
 514                 out->m[1][3] = y;
 515                 out->m[2][0] = 0;
 516                 out->m[2][1] = 0;
 517                 out->m[2][2] = scale;
 518                 out->m[2][3] = z;
 519                 out->m[3][0] = 0;
 520                 out->m[3][1] = 0;
 521                 out->m[3][2] = 0;
 522                 out->m[3][3] = 1;
 523 #endif
 524         }
 525 }
 526
 527 void Matrix4x4_ToVectors(const matrix4x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
 528 {
 529 #ifdef MATRIX4x4_OPENGLORIENTATION
 530         vx[0] = in->m[0][0];
 531         vx[1] = in->m[0][1];
 532         vx[2] = in->m[0][2];
 533         vy[0] = in->m[1][0];
 534         vy[1] = in->m[1][1];
 535         vy[2] = in->m[1][2];
 536         vz[0] = in->m[2][0];
 537         vz[1] = in->m[2][1];
 538         vz[2] = in->m[2][2];
 539         t [0] = in->m[3][0];
 540         t [1] = in->m[3][1];
 541         t [2] = in->m[3][2];
 542 #else
 543         vx[0] = in->m[0][0];
 544         vx[1] = in->m[1][0];
 545         vx[2] = in->m[2][0];
 546         vy[0] = in->m[0][1];
 547         vy[1] = in->m[1][1];
 548         vy[2] = in->m[2][1];
 549         vz[0] = in->m[0][2];
 550         vz[1] = in->m[1][2];
 551         vz[2] = in->m[2][2];
 552         t [0] = in->m[0][3];
 553         t [1] = in->m[1][3];
 554         t [2] = in->m[2][3];
 555 #endif
 556 }
 557
 558 void Matrix4x4_FromVectors(matrix4x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
 559 {
 560 #ifdef MATRIX4x4_OPENGLORIENTATION
 561         out->m[0][0] = vx[0];
 562         out->m[1][0] = vy[0];
 563         out->m[2][0] = vz[0];
 564         out->m[3][0] = t[0];
 565         out->m[0][1] = vx[1];
 566         out->m[1][1] = vy[1];
 567         out->m[2][1] = vz[1];
 568         out->m[3][1] = t[1];
 569         out->m[0][2] = vx[2];
 570         out->m[1][2] = vy[2];
 571         out->m[2][2] = vz[2];
 572         out->m[3][2] = t[2];
 573         out->m[0][3] = 0.0f;
 574         out->m[1][3] = 0.0f;
 575         out->m[2][3] = 0.0f;
 576         out->m[3][3] = 1.0f;
 577 #else
 578         out->m[0][0] = vx[0];
 579         out->m[0][1] = vy[0];
 580         out->m[0][2] = vz[0];
 581         out->m[0][3] = t[0];
 582         out->m[1][0] = vx[1];
 583         out->m[1][1] = vy[1];
 584         out->m[1][2] = vz[1];
 585         out->m[1][3] = t[1];
 586         out->m[2][0] = vx[2];
 587         out->m[2][1] = vy[2];
 588         out->m[2][2] = vz[2];
 589         out->m[2][3] = t[2];
 590         out->m[3][0] = 0.0f;
 591         out->m[3][1] = 0.0f;
 592         out->m[3][2] = 0.0f;
 593         out->m[3][3] = 1.0f;
 594 #endif
 595 }
 596
 597 void Matrix4x4_ToArrayDoubleGL(const matrix4x4_t *in, double out[16])
 598 {
 599 #ifdef MATRIX4x4_OPENGLORIENTATION
 600         out[ 0] = in->m[0][0];
 601         out[ 1] = in->m[0][1];
 602         out[ 2] = in->m[0][2];
 603         out[ 3] = in->m[0][3];
 604         out[ 4] = in->m[1][0];
 605         out[ 5] = in->m[1][1];
 606         out[ 6] = in->m[1][2];
 607         out[ 7] = in->m[1][3];
 608         out[ 8] = in->m[2][0];
 609         out[ 9] = in->m[2][1];
 610         out[10] = in->m[2][2];
 611         out[11] = in->m[2][3];
 612         out[12] = in->m[3][0];
 613         out[13] = in->m[3][1];
 614         out[14] = in->m[3][2];
 615         out[15] = in->m[3][3];
 616 #else
 617         out[ 0] = in->m[0][0];
 618         out[ 1] = in->m[1][0];
 619         out[ 2] = in->m[2][0];
 620         out[ 3] = in->m[3][0];
 621         out[ 4] = in->m[0][1];
 622         out[ 5] = in->m[1][1];
 623         out[ 6] = in->m[2][1];
 624         out[ 7] = in->m[3][1];
 625         out[ 8] = in->m[0][2];
 626         out[ 9] = in->m[1][2];
 627         out[10] = in->m[2][2];
 628         out[11] = in->m[3][2];
 629         out[12] = in->m[0][3];
 630         out[13] = in->m[1][3];
 631         out[14] = in->m[2][3];
 632         out[15] = in->m[3][3];
 633 #endif
 634 }
 635
 636 void Matrix4x4_FromArrayDoubleGL (matrix4x4_t *out, const double in[16])
 637 {
 638 #ifdef MATRIX4x4_OPENGLORIENTATION
 639         out->m[0][0] = in[0];
 640         out->m[0][1] = in[1];
 641         out->m[0][2] = in[2];
 642         out->m[0][3] = in[3];
 643         out->m[1][0] = in[4];
 644         out->m[1][1] = in[5];
 645         out->m[1][2] = in[6];
 646         out->m[1][3] = in[7];
 647         out->m[2][0] = in[8];
 648         out->m[2][1] = in[9];
 649         out->m[2][2] = in[10];
 650         out->m[2][3] = in[11];
 651         out->m[3][0] = in[12];
 652         out->m[3][1] = in[13];
 653         out->m[3][2] = in[14];
 654         out->m[3][3] = in[15];
 655 #else
 656         out->m[0][0] = in[0];
 657         out->m[1][0] = in[1];
 658         out->m[2][0] = in[2];
 659         out->m[3][0] = in[3];
 660         out->m[0][1] = in[4];
 661         out->m[1][1] = in[5];
 662         out->m[2][1] = in[6];
 663         out->m[3][1] = in[7];
 664         out->m[0][2] = in[8];
 665         out->m[1][2] = in[9];
 666         out->m[2][2] = in[10];
 667         out->m[3][2] = in[11];
 668         out->m[0][3] = in[12];
 669         out->m[1][3] = in[13];
 670         out->m[2][3] = in[14];
 671         out->m[3][3] = in[15];
 672 #endif
 673 }
 674
 675 void Matrix4x4_ToArrayDoubleD3D(const matrix4x4_t *in, double out[16])
 676 {
 677 #ifdef MATRIX4x4_OPENGLORIENTATION
 678         out[ 0] = in->m[0][0];
 679         out[ 1] = in->m[1][0];
 680         out[ 2] = in->m[2][0];
 681         out[ 3] = in->m[3][0];
 682         out[ 4] = in->m[0][1];
 683         out[ 5] = in->m[1][1];
 684         out[ 6] = in->m[2][1];
 685         out[ 7] = in->m[3][1];
 686         out[ 8] = in->m[0][2];
 687         out[ 9] = in->m[1][2];
 688         out[10] = in->m[2][2];
 689         out[11] = in->m[3][2];
 690         out[12] = in->m[0][3];
 691         out[13] = in->m[1][3];
 692         out[14] = in->m[2][3];
 693         out[15] = in->m[3][3];
 694 #else
 695         out[ 0] = in->m[0][0];
 696         out[ 1] = in->m[0][1];
 697         out[ 2] = in->m[0][2];
 698         out[ 3] = in->m[0][3];
 699         out[ 4] = in->m[1][0];
 700         out[ 5] = in->m[1][1];
 701         out[ 6] = in->m[1][2];
 702         out[ 7] = in->m[1][3];
 703         out[ 8] = in->m[2][0];
 704         out[ 9] = in->m[2][1];
 705         out[10] = in->m[2][2];
 706         out[11] = in->m[2][3];
 707         out[12] = in->m[3][0];
 708         out[13] = in->m[3][1];
 709         out[14] = in->m[3][2];
 710         out[15] = in->m[3][3];
 711 #endif
 712 }
 713
 714 void Matrix4x4_FromArrayDoubleD3D (matrix4x4_t *out, const double in[16])
 715 {
 716 #ifdef MATRIX4x4_OPENGLORIENTATION
 717         out->m[0][0] = in[0];
 718         out->m[1][0] = in[1];
 719         out->m[2][0] = in[2];
 720         out->m[3][0] = in[3];
 721         out->m[0][1] = in[4];
 722         out->m[1][1] = in[5];
 723         out->m[2][1] = in[6];
 724         out->m[3][1] = in[7];
 725         out->m[0][2] = in[8];
 726         out->m[1][2] = in[9];
 727         out->m[2][2] = in[10];
 728         out->m[3][2] = in[11];
 729         out->m[0][3] = in[12];
 730         out->m[1][3] = in[13];
 731         out->m[2][3] = in[14];
 732         out->m[3][3] = in[15];
 733 #else
 734         out->m[0][0] = in[0];
 735         out->m[0][1] = in[1];
 736         out->m[0][2] = in[2];
 737         out->m[0][3] = in[3];
 738         out->m[1][0] = in[4];
 739         out->m[1][1] = in[5];
 740         out->m[1][2] = in[6];
 741         out->m[1][3] = in[7];
 742         out->m[2][0] = in[8];
 743         out->m[2][1] = in[9];
 744         out->m[2][2] = in[10];
 745         out->m[2][3] = in[11];
 746         out->m[3][0] = in[12];
 747         out->m[3][1] = in[13];
 748         out->m[3][2] = in[14];
 749         out->m[3][3] = in[15];
 750 #endif
 751 }
 752
 753 void Matrix4x4_ToArray12FloatGL(const matrix4x4_t *in, float out[12])
 754 {
 755 #ifdef MATRIX4x4_OPENGLORIENTATION
 756         out[ 0] = in->m[0][0];
 757         out[ 1] = in->m[0][1];
 758         out[ 2] = in->m[0][2];
 759         out[ 3] = in->m[1][0];
 760         out[ 4] = in->m[1][1];
 761         out[ 5] = in->m[1][2];
 762         out[ 6] = in->m[2][0];
 763         out[ 7] = in->m[2][1];
 764         out[ 8] = in->m[2][2];
 765         out[ 9] = in->m[3][0];
 766         out[10] = in->m[3][1];
 767         out[11] = in->m[3][2];
 768 #else
 769         out[ 0] = in->m[0][0];
 770         out[ 1] = in->m[1][0];
 771         out[ 2] = in->m[2][0];
 772         out[ 3] = in->m[0][1];
 773         out[ 4] = in->m[1][1];
 774         out[ 5] = in->m[2][1];
 775         out[ 6] = in->m[0][2];
 776         out[ 7] = in->m[1][2];
 777         out[ 8] = in->m[2][2];
 778         out[ 9] = in->m[0][3];
 779         out[10] = in->m[1][3];
 780         out[11] = in->m[2][3];
 781 #endif
 782 }
 783
 784 void Matrix4x4_FromArray12FloatGL(matrix4x4_t *out, const float in[12])
 785 {
 786 #ifdef MATRIX4x4_OPENGLORIENTATION
 787         out->m[0][0] = in[0];
 788         out->m[0][1] = in[1];
 789         out->m[0][2] = in[2];
 790         out->m[0][3] = 0;
 791         out->m[1][0] = in[3];
 792         out->m[1][1] = in[4];
 793         out->m[1][2] = in[5];
 794         out->m[1][3] = 0;
 795         out->m[2][0] = in[6];
 796         out->m[2][1] = in[7];
 797         out->m[2][2] = in[8];
 798         out->m[2][3] = 0;
 799         out->m[3][0] = in[9];
 800         out->m[3][1] = in[10];
 801         out->m[3][2] = in[11];
 802         out->m[3][3] = 1;
 803 #else
 804         out->m[0][0] = in[0];
 805         out->m[1][0] = in[1];
 806         out->m[2][0] = in[2];
 807         out->m[3][0] = 0;
 808         out->m[0][1] = in[3];
 809         out->m[1][1] = in[4];
 810         out->m[2][1] = in[5];
 811         out->m[3][1] = 0;
 812         out->m[0][2] = in[6];
 813         out->m[1][2] = in[7];
 814         out->m[2][2] = in[8];
 815         out->m[3][2] = 0;
 816         out->m[0][3] = in[9];
 817         out->m[1][3] = in[10];
 818         out->m[2][3] = in[11];
 819         out->m[3][3] = 1;
 820 #endif
 821 }
 822
 823 void Matrix4x4_ToArray12FloatD3D(const matrix4x4_t *in, float out[12])
 824 {
 825 #ifdef MATRIX4x4_OPENGLORIENTATION
 826         out[ 0] = in->m[0][0];
 827         out[ 1] = in->m[1][0];
 828         out[ 2] = in->m[2][0];
 829         out[ 3] = in->m[3][0];
 830         out[ 4] = in->m[0][1];
 831         out[ 5] = in->m[1][1];
 832         out[ 6] = in->m[2][1];
 833         out[ 7] = in->m[3][1];
 834         out[ 8] = in->m[0][2];
 835         out[ 9] = in->m[1][2];
 836         out[10] = in->m[2][2];
 837         out[11] = in->m[3][2];
 838 #else
 839         out[ 0] = in->m[0][0];
 840         out[ 1] = in->m[0][1];
 841         out[ 2] = in->m[0][2];
 842         out[ 3] = in->m[0][3];
 843         out[ 4] = in->m[1][0];
 844         out[ 5] = in->m[1][1];
 845         out[ 6] = in->m[1][2];
 846         out[ 7] = in->m[1][3];
 847         out[ 8] = in->m[2][0];
 848         out[ 9] = in->m[2][1];
 849         out[10] = in->m[2][2];
 850         out[11] = in->m[2][3];
 851 #endif
 852 }
 853
 854 void Matrix4x4_FromArray12FloatD3D(matrix4x4_t *out, const float in[12])
 855 {
 856 #ifdef MATRIX4x4_OPENGLORIENTATION
 857         out->m[0][0] = in[0];
 858         out->m[1][0] = in[1];
 859         out->m[2][0] = in[2];
 860         out->m[3][0] = in[3];
 861         out->m[0][1] = in[4];
 862         out->m[1][1] = in[5];
 863         out->m[2][1] = in[6];
 864         out->m[3][1] = in[7];
 865         out->m[0][2] = in[8];
 866         out->m[1][2] = in[9];
 867         out->m[2][2] = in[10];
 868         out->m[3][2] = in[11];
 869         out->m[0][3] = 0;
 870         out->m[1][3] = 0;
 871         out->m[2][3] = 0;
 872         out->m[3][3] = 1;
 873 #else
 874         out->m[0][0] = in[0];
 875         out->m[0][1] = in[1];
 876         out->m[0][2] = in[2];
 877         out->m[0][3] = in[3];
 878         out->m[1][0] = in[4];
 879         out->m[1][1] = in[5];
 880         out->m[1][2] = in[6];
 881         out->m[1][3] = in[7];
 882         out->m[2][0] = in[8];
 883         out->m[2][1] = in[9];
 884         out->m[2][2] = in[10];
 885         out->m[2][3] = in[11];
 886         out->m[3][0] = 0;
 887         out->m[3][1] = 0;
 888         out->m[3][2] = 0;
 889         out->m[3][3] = 1;
 890 #endif
 891 }
 892
 893 void Matrix4x4_FromOriginQuat(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z, double w)
 894 {
 895 #ifdef MATRIX4x4_OPENGLORIENTATION
 896         m->m[0][0]=1-2*(y*y+z*z);m->m[1][0]=  2*(x*y-z*w);m->m[2][0]=  2*(x*z+y*w);m->m[3][0]=ox;
 897         m->m[0][1]=  2*(x*y+z*w);m->m[1][1]=1-2*(x*x+z*z);m->m[2][1]=  2*(y*z-x*w);m->m[3][1]=oy;
 898         m->m[0][2]=  2*(x*z-y*w);m->m[1][2]=  2*(y*z+x*w);m->m[2][2]=1-2*(x*x+y*y);m->m[3][2]=oz;
 899         m->m[0][3]=  0          ;m->m[1][3]=  0          ;m->m[2][3]=  0          ;m->m[3][3]=1;
 900 #else
 901         m->m[0][0]=1-2*(y*y+z*z);m->m[0][1]=  2*(x*y-z*w);m->m[0][2]=  2*(x*z+y*w);m->m[0][3]=ox;
 902         m->m[1][0]=  2*(x*y+z*w);m->m[1][1]=1-2*(x*x+z*z);m->m[1][2]=  2*(y*z-x*w);m->m[1][3]=oy;
 903         m->m[2][0]=  2*(x*z-y*w);m->m[2][1]=  2*(y*z+x*w);m->m[2][2]=1-2*(x*x+y*y);m->m[2][3]=oz;
 904         m->m[3][0]=  0          ;m->m[3][1]=  0          ;m->m[3][2]=  0          ;m->m[3][3]=1;
 905 #endif
 906 }
 907
 908 // LordHavoc: I got this code from:
 909 //http://www.doom3world.org/phpbb2/viewtopic.php?t=2884
 910 void Matrix4x4_FromDoom3Joint(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z)
 911 {
 912         double w = 1.0 - (x*x+y*y+z*z);
 913         w = w > 0.0 ? -sqrt(w) : 0.0;
 914 #ifdef MATRIX4x4_OPENGLORIENTATION
 915         m->m[0][0]=1-2*(y*y+z*z);m->m[1][0]=  2*(x*y-z*w);m->m[2][0]=  2*(x*z+y*w);m->m[3][0]=ox;
 916         m->m[0][1]=  2*(x*y+z*w);m->m[1][1]=1-2*(x*x+z*z);m->m[2][1]=  2*(y*z-x*w);m->m[3][1]=oy;
 917         m->m[0][2]=  2*(x*z-y*w);m->m[1][2]=  2*(y*z+x*w);m->m[2][2]=1-2*(x*x+y*y);m->m[3][2]=oz;
 918         m->m[0][3]=  0          ;m->m[1][3]=  0          ;m->m[2][3]=  0          ;m->m[3][3]=1;
 919 #else
 920         m->m[0][0]=1-2*(y*y+z*z);m->m[0][1]=  2*(x*y-z*w);m->m[0][2]=  2*(x*z+y*w);m->m[0][3]=ox;
 921         m->m[1][0]=  2*(x*y+z*w);m->m[1][1]=1-2*(x*x+z*z);m->m[1][2]=  2*(y*z-x*w);m->m[1][3]=oy;
 922         m->m[2][0]=  2*(x*z-y*w);m->m[2][1]=  2*(y*z+x*w);m->m[2][2]=1-2*(x*x+y*y);m->m[2][3]=oz;
 923         m->m[3][0]=  0          ;m->m[3][1]=  0          ;m->m[3][2]=  0          ;m->m[3][3]=1;
 924 #endif
 925 }
 926
 927 void Matrix4x4_Blend (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2, double blend)
 928 {
 929         double iblend = 1 - blend;
 930         out->m[0][0] = in1->m[0][0] * iblend + in2->m[0][0] * blend;
 931         out->m[0][1] = in1->m[0][1] * iblend + in2->m[0][1] * blend;
 932         out->m[0][2] = in1->m[0][2] * iblend + in2->m[0][2] * blend;
 933         out->m[0][3] = in1->m[0][3] * iblend + in2->m[0][3] * blend;
 934         out->m[1][0] = in1->m[1][0] * iblend + in2->m[1][0] * blend;
 935         out->m[1][1] = in1->m[1][1] * iblend + in2->m[1][1] * blend;
 936         out->m[1][2] = in1->m[1][2] * iblend + in2->m[1][2] * blend;
 937         out->m[1][3] = in1->m[1][3] * iblend + in2->m[1][3] * blend;
 938         out->m[2][0] = in1->m[2][0] * iblend + in2->m[2][0] * blend;
 939         out->m[2][1] = in1->m[2][1] * iblend + in2->m[2][1] * blend;
 940         out->m[2][2] = in1->m[2][2] * iblend + in2->m[2][2] * blend;
 941         out->m[2][3] = in1->m[2][3] * iblend + in2->m[2][3] * blend;
 942         out->m[3][0] = in1->m[3][0] * iblend + in2->m[3][0] * blend;
 943         out->m[3][1] = in1->m[3][1] * iblend + in2->m[3][1] * blend;
 944         out->m[3][2] = in1->m[3][2] * iblend + in2->m[3][2] * blend;
 945         out->m[3][3] = in1->m[3][3] * iblend + in2->m[3][3] * blend;
 946 }
 947
 948
 949 void Matrix4x4_Transform (const matrix4x4_t *in, const float v[3], float out[3])
 950 {
 951 #ifdef MATRIX4x4_OPENGLORIENTATION
 952         out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0] + in->m[3][0];
 953         out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1] + in->m[3][1];
 954         out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2] + in->m[3][2];
 955 #else
 956         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
 957         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
 958         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
 959 #endif
 960 }
 961
 962 void Matrix4x4_Transform4 (const matrix4x4_t *in, const float v[4], float out[4])
 963 {
 964 #ifdef MATRIX4x4_OPENGLORIENTATION
 965         out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0] + v[3] * in->m[3][0];
 966         out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1] + v[3] * in->m[3][1];
 967         out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2] + v[3] * in->m[3][2];
 968         out[3] = v[0] * in->m[0][3] + v[1] * in->m[1][3] + v[2] * in->m[2][3] + v[3] * in->m[3][3];
 969 #else
 970         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + v[3] * in->m[0][3];
 971         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + v[3] * in->m[1][3];
 972         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + v[3] * in->m[2][3];
 973         out[3] = v[0] * in->m[3][0] + v[1] * in->m[3][1] + v[2] * in->m[3][2] + v[3] * in->m[3][3];
 974 #endif
 975 }
 976
 977 void Matrix4x4_Transform3x3 (const matrix4x4_t *in, const float v[3], float out[3])
 978 {
 979 #ifdef MATRIX4x4_OPENGLORIENTATION
 980         out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0];
 981         out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1];
 982         out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2];
 983 #else
 984         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2];
 985         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2];
 986         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2];
 987 #endif
 988 }
 989
 990 /*
 991 void Matrix4x4_SimpleUntransform (const matrix4x4_t *in, const float v[3], float out[3])
 992 {
 993         double t[3];
 994 #ifdef MATRIX4x4_OPENGLORIENTATION
 995         t[0] = v[0] - in->m[3][0];
 996         t[1] = v[1] - in->m[3][1];
 997         t[2] = v[2] - in->m[3][2];
 998         out[0] = t[0] * in->m[0][0] + t[1] * in->m[0][1] + t[2] * in->m[0][2];
 999         out[1] = t[0] * in->m[1][0] + t[1] * in->m[1][1] + t[2] * in->m[1][2];
1000         out[2] = t[0] * in->m[2][0] + t[1] * in->m[2][1] + t[2] * in->m[2][2];
1001 #else
1002         t[0] = v[0] - in->m[0][3];
1003         t[1] = v[1] - in->m[1][3];
1004         t[2] = v[2] - in->m[2][3];
1005         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
1006         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
1007         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
1008 #endif
1009 }
1010 */
1011
1012 // FIXME: optimize
1013 void Matrix4x4_ConcatTranslate (matrix4x4_t *out, double x, double y, double z)
1014 {
1015         matrix4x4_t base, temp;
1016         base = *out;
1017         Matrix4x4_CreateTranslate(&temp, x, y, z);
1018         Matrix4x4_Concat(out, &base, &temp);
1019 }
1020
1021 // FIXME: optimize
1022 void Matrix4x4_ConcatRotate (matrix4x4_t *out, double angle, double x, double y, double z)
1023 {
1024         matrix4x4_t base, temp;
1025         base = *out;
1026         Matrix4x4_CreateRotate(&temp, angle, x, y, z);
1027         Matrix4x4_Concat(out, &base, &temp);
1028 }
1029
1030 // FIXME: optimize
1031 void Matrix4x4_ConcatScale (matrix4x4_t *out, double x)
1032 {
1033         matrix4x4_t base, temp;
1034         base = *out;
1035         Matrix4x4_CreateScale(&temp, x);
1036         Matrix4x4_Concat(out, &base, &temp);
1037 }
1038
1039 // FIXME: optimize
1040 void Matrix4x4_ConcatScale3 (matrix4x4_t *out, double x, double y, double z)
1041 {
1042         matrix4x4_t base, temp;
1043         base = *out;
1044         Matrix4x4_CreateScale3(&temp, x, y, z);
1045         Matrix4x4_Concat(out, &base, &temp);
1046 }
1047
1048 void Matrix4x4_OriginFromMatrix (const matrix4x4_t *in, float *out)
1049 {
1050 #ifdef MATRIX4x4_OPENGLORIENTATION
1051         out[0] = in->m[3][0];
1052         out[1] = in->m[3][1];
1053         out[2] = in->m[3][2];
1054 #else
1055         out[0] = in->m[0][3];
1056         out[1] = in->m[1][3];
1057         out[2] = in->m[2][3];
1058 #endif
1059 }
1060
1061 double Matrix4x4_ScaleFromMatrix (const matrix4x4_t *in)
1062 {
1063         // we only support uniform scaling, so assume the first row is enough
1064         return sqrt(in->m[0][0] * in->m[0][0] + in->m[0][1] * in->m[0][1] + in->m[0][2] * in->m[0][2]);
1065 }
1066
1067 void Matrix4x4_SetOrigin (matrix4x4_t *out, double x, double y, double z)
1068 {
1069 #ifdef MATRIX4x4_OPENGLORIENTATION
1070         out->m[3][0] = x;
1071         out->m[3][1] = y;
1072         out->m[3][2] = z;
1073 #else
1074         out->m[0][3] = x;
1075         out->m[1][3] = y;
1076         out->m[2][3] = z;
1077 #endif
1078 }
1079
1080 void Matrix4x4_AdjustOrigin (matrix4x4_t *out, double x, double y, double z)
1081 {
1082 #ifdef MATRIX4x4_OPENGLORIENTATION
1083         out->m[3][0] += x;
1084         out->m[3][1] += y;
1085         out->m[3][2] += z;
1086 #else
1087         out->m[0][3] += x;
1088         out->m[1][3] += y;
1089         out->m[2][3] += z;
1090 #endif
1091 }
1092
1093 void Matrix4x4_Scale (matrix4x4_t *out, double rotatescale, double originscale)
1094 {
1095         out->m[0][0] *= rotatescale;
1096         out->m[0][1] *= rotatescale;
1097         out->m[0][2] *= rotatescale;
1098         out->m[1][0] *= rotatescale;
1099         out->m[1][1] *= rotatescale;
1100         out->m[1][2] *= rotatescale;
1101         out->m[2][0] *= rotatescale;
1102         out->m[2][1] *= rotatescale;
1103         out->m[2][2] *= rotatescale;
1104 #ifdef MATRIX4x4_OPENGLORIENTATION
1105         out->m[3][0] *= originscale;
1106         out->m[3][1] *= originscale;
1107         out->m[3][2] *= originscale;
1108 #else
1109         out->m[0][3] *= originscale;
1110         out->m[1][3] *= originscale;
1111         out->m[2][3] *= originscale;
1112 #endif
1113 }