]> git.xonotic.org Git - xonotic/darkplaces.git/blob - matrixlib.c
model_shared: Fix compile warning and error with gcc
[xonotic/darkplaces.git] / matrixlib.c
1 #include "darkplaces.h"
2 #include <math.h>
3
4 #ifdef _MSC_VER
5 #pragma warning(disable : 4244)     // LadyHavoc: MSVC++ 4 x86, double/float
6 #pragma warning(disable : 4305)         // LadyHavoc: MSVC++ 6 x86, double/float
7 #endif
8
9 const matrix4x4_t identitymatrix =
10 {
11         {
12                 {1, 0, 0, 0},
13                 {0, 1, 0, 0},
14                 {0, 0, 1, 0},
15                 {0, 0, 0, 1}
16         }
17 };
18
19 void Matrix4x4_Copy (matrix4x4_t *out, const matrix4x4_t *in)
20 {
21         *out = *in;
22 }
23
24 void Matrix4x4_CopyRotateOnly (matrix4x4_t *out, const matrix4x4_t *in)
25 {
26         out->m[0][0] = in->m[0][0];
27         out->m[0][1] = in->m[0][1];
28         out->m[0][2] = in->m[0][2];
29         out->m[0][3] = 0.0f;
30         out->m[1][0] = in->m[1][0];
31         out->m[1][1] = in->m[1][1];
32         out->m[1][2] = in->m[1][2];
33         out->m[1][3] = 0.0f;
34         out->m[2][0] = in->m[2][0];
35         out->m[2][1] = in->m[2][1];
36         out->m[2][2] = in->m[2][2];
37         out->m[2][3] = 0.0f;
38         out->m[3][0] = 0.0f;
39         out->m[3][1] = 0.0f;
40         out->m[3][2] = 0.0f;
41         out->m[3][3] = 1.0f;
42 }
43
44 void Matrix4x4_CopyTranslateOnly (matrix4x4_t *out, const matrix4x4_t *in)
45 {
46 #ifdef MATRIX4x4_OPENGLORIENTATION
47         out->m[0][0] = 1.0f;
48         out->m[1][0] = 0.0f;
49         out->m[2][0] = 0.0f;
50         out->m[3][0] = in->m[0][3];
51         out->m[0][1] = 0.0f;
52         out->m[1][1] = 1.0f;
53         out->m[2][1] = 0.0f;
54         out->m[3][1] = in->m[1][3];
55         out->m[0][2] = 0.0f;
56         out->m[1][2] = 0.0f;
57         out->m[2][2] = 1.0f;
58         out->m[3][2] = in->m[2][3];
59         out->m[0][3] = 0.0f;
60         out->m[1][3] = 0.0f;
61         out->m[2][3] = 0.0f;
62         out->m[3][3] = 1.0f;
63 #else
64         out->m[0][0] = 1.0f;
65         out->m[0][1] = 0.0f;
66         out->m[0][2] = 0.0f;
67         out->m[0][3] = in->m[0][3];
68         out->m[1][0] = 0.0f;
69         out->m[1][1] = 1.0f;
70         out->m[1][2] = 0.0f;
71         out->m[1][3] = in->m[1][3];
72         out->m[2][0] = 0.0f;
73         out->m[2][1] = 0.0f;
74         out->m[2][2] = 1.0f;
75         out->m[2][3] = in->m[2][3];
76         out->m[3][0] = 0.0f;
77         out->m[3][1] = 0.0f;
78         out->m[3][2] = 0.0f;
79         out->m[3][3] = 1.0f;
80 #endif
81 }
82
83 void Matrix4x4_Concat (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2)
84 {
85 #ifdef MATRIX4x4_OPENGLORIENTATION
86         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];
87         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];
88         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];
89         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];
90         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];
91         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];
92         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];
93         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];
94         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];
95         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];
96         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];
97         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];
98         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];
99         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];
100         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];
101         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];
102 #else
103         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];
104         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];
105         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];
106         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];
107         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];
108         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];
109         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];
110         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];
111         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];
112         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];
113         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];
114         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];
115         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];
116         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];
117         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];
118         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];
119 #endif
120 }
121
122 void Matrix4x4_Transpose (matrix4x4_t *out, const matrix4x4_t *in1)
123 {
124         out->m[0][0] = in1->m[0][0];
125         out->m[0][1] = in1->m[1][0];
126         out->m[0][2] = in1->m[2][0];
127         out->m[0][3] = in1->m[3][0];
128         out->m[1][0] = in1->m[0][1];
129         out->m[1][1] = in1->m[1][1];
130         out->m[1][2] = in1->m[2][1];
131         out->m[1][3] = in1->m[3][1];
132         out->m[2][0] = in1->m[0][2];
133         out->m[2][1] = in1->m[1][2];
134         out->m[2][2] = in1->m[2][2];
135         out->m[2][3] = in1->m[3][2];
136         out->m[3][0] = in1->m[0][3];
137         out->m[3][1] = in1->m[1][3];
138         out->m[3][2] = in1->m[2][3];
139         out->m[3][3] = in1->m[3][3];
140 }
141
142 #if 1
143 // Adapted from code contributed to Mesa by David Moore (Mesa 7.6 under SGI Free License B - which is MIT/X11-type)
144 // added helper for common subexpression elimination by eihrul, and other optimizations by div0
145 int Matrix4x4_Invert_Full (matrix4x4_t *out, const matrix4x4_t *in1)
146 {
147                 float det;
148
149                 // note: orientation does not matter, as transpose(invert(transpose(m))) == invert(m), proof:
150                 //   transpose(invert(transpose(m))) * m
151                 // = transpose(invert(transpose(m))) * transpose(transpose(m))
152                 // = transpose(transpose(m) * invert(transpose(m)))
153                 // = transpose(identity)
154                 // = identity
155
156                 // this seems to help gcc's common subexpression elimination, and also makes the code look nicer
157                 float   m00 = in1->m[0][0], m01 = in1->m[0][1], m02 = in1->m[0][2], m03 = in1->m[0][3],
158                                 m10 = in1->m[1][0], m11 = in1->m[1][1], m12 = in1->m[1][2], m13 = in1->m[1][3],
159                                 m20 = in1->m[2][0], m21 = in1->m[2][1], m22 = in1->m[2][2], m23 = in1->m[2][3],
160                                 m30 = in1->m[3][0], m31 = in1->m[3][1], m32 = in1->m[3][2], m33 = in1->m[3][3];
161
162                 // calculate the adjoint
163                 out->m[0][0] =  (m11*(m22*m33 - m23*m32) - m21*(m12*m33 - m13*m32) + m31*(m12*m23 - m13*m22));
164                 out->m[0][1] = -(m01*(m22*m33 - m23*m32) - m21*(m02*m33 - m03*m32) + m31*(m02*m23 - m03*m22));
165                 out->m[0][2] =  (m01*(m12*m33 - m13*m32) - m11*(m02*m33 - m03*m32) + m31*(m02*m13 - m03*m12));
166                 out->m[0][3] = -(m01*(m12*m23 - m13*m22) - m11*(m02*m23 - m03*m22) + m21*(m02*m13 - m03*m12));
167                 out->m[1][0] = -(m10*(m22*m33 - m23*m32) - m20*(m12*m33 - m13*m32) + m30*(m12*m23 - m13*m22));
168                 out->m[1][1] =  (m00*(m22*m33 - m23*m32) - m20*(m02*m33 - m03*m32) + m30*(m02*m23 - m03*m22));
169                 out->m[1][2] = -(m00*(m12*m33 - m13*m32) - m10*(m02*m33 - m03*m32) + m30*(m02*m13 - m03*m12));
170                 out->m[1][3] =  (m00*(m12*m23 - m13*m22) - m10*(m02*m23 - m03*m22) + m20*(m02*m13 - m03*m12));
171                 out->m[2][0] =  (m10*(m21*m33 - m23*m31) - m20*(m11*m33 - m13*m31) + m30*(m11*m23 - m13*m21));
172                 out->m[2][1] = -(m00*(m21*m33 - m23*m31) - m20*(m01*m33 - m03*m31) + m30*(m01*m23 - m03*m21));
173                 out->m[2][2] =  (m00*(m11*m33 - m13*m31) - m10*(m01*m33 - m03*m31) + m30*(m01*m13 - m03*m11));
174                 out->m[2][3] = -(m00*(m11*m23 - m13*m21) - m10*(m01*m23 - m03*m21) + m20*(m01*m13 - m03*m11));
175                 out->m[3][0] = -(m10*(m21*m32 - m22*m31) - m20*(m11*m32 - m12*m31) + m30*(m11*m22 - m12*m21));
176                 out->m[3][1] =  (m00*(m21*m32 - m22*m31) - m20*(m01*m32 - m02*m31) + m30*(m01*m22 - m02*m21));
177                 out->m[3][2] = -(m00*(m11*m32 - m12*m31) - m10*(m01*m32 - m02*m31) + m30*(m01*m12 - m02*m11));
178                 out->m[3][3] =  (m00*(m11*m22 - m12*m21) - m10*(m01*m22 - m02*m21) + m20*(m01*m12 - m02*m11));
179
180                 // calculate the determinant (as inverse == 1/det * adjoint, adjoint * m == identity * det, so this calculates the det)
181                 det = m00*out->m[0][0] + m10*out->m[0][1] + m20*out->m[0][2] + m30*out->m[0][3];
182                 if (det == 0.0f)
183                                 return 0;
184
185                 // multiplications are faster than divisions, usually
186                 det = 1.0f / det;
187
188                 // manually unrolled loop to multiply all matrix elements by 1/det
189                 out->m[0][0] *= det; out->m[0][1] *= det; out->m[0][2] *= det; out->m[0][3] *= det;
190                 out->m[1][0] *= det; out->m[1][1] *= det; out->m[1][2] *= det; out->m[1][3] *= det;
191                 out->m[2][0] *= det; out->m[2][1] *= det; out->m[2][2] *= det; out->m[2][3] *= det;
192                 out->m[3][0] *= det; out->m[3][1] *= det; out->m[3][2] *= det; out->m[3][3] *= det;
193
194                 return 1;
195 }
196 #elif 1
197 // Adapted from code contributed to Mesa by David Moore (Mesa 7.6 under SGI Free License B - which is MIT/X11-type)
198 int Matrix4x4_Invert_Full (matrix4x4_t *out, const matrix4x4_t *in1)
199 {
200         matrix4x4_t temp;
201         double det;
202         int i, j;
203
204 #ifdef MATRIX4x4_OPENGLORIENTATION
205         temp.m[0][0] =  in1->m[1][1]*in1->m[2][2]*in1->m[3][3] - in1->m[1][1]*in1->m[2][3]*in1->m[3][2] - in1->m[2][1]*in1->m[1][2]*in1->m[3][3] + in1->m[2][1]*in1->m[1][3]*in1->m[3][2] + in1->m[3][1]*in1->m[1][2]*in1->m[2][3] - in1->m[3][1]*in1->m[1][3]*in1->m[2][2];
206         temp.m[1][0] = -in1->m[1][0]*in1->m[2][2]*in1->m[3][3] + in1->m[1][0]*in1->m[2][3]*in1->m[3][2] + in1->m[2][0]*in1->m[1][2]*in1->m[3][3] - in1->m[2][0]*in1->m[1][3]*in1->m[3][2] - in1->m[3][0]*in1->m[1][2]*in1->m[2][3] + in1->m[3][0]*in1->m[1][3]*in1->m[2][2];
207         temp.m[2][0] =  in1->m[1][0]*in1->m[2][1]*in1->m[3][3] - in1->m[1][0]*in1->m[2][3]*in1->m[3][1] - in1->m[2][0]*in1->m[1][1]*in1->m[3][3] + in1->m[2][0]*in1->m[1][3]*in1->m[3][1] + in1->m[3][0]*in1->m[1][1]*in1->m[2][3] - in1->m[3][0]*in1->m[1][3]*in1->m[2][1];
208         temp.m[3][0] = -in1->m[1][0]*in1->m[2][1]*in1->m[3][2] + in1->m[1][0]*in1->m[2][2]*in1->m[3][1] + in1->m[2][0]*in1->m[1][1]*in1->m[3][2] - in1->m[2][0]*in1->m[1][2]*in1->m[3][1] - in1->m[3][0]*in1->m[1][1]*in1->m[2][2] + in1->m[3][0]*in1->m[1][2]*in1->m[2][1];
209         temp.m[0][1] = -in1->m[0][1]*in1->m[2][2]*in1->m[3][3] + in1->m[0][1]*in1->m[2][3]*in1->m[3][2] + in1->m[2][1]*in1->m[0][2]*in1->m[3][3] - in1->m[2][1]*in1->m[0][3]*in1->m[3][2] - in1->m[3][1]*in1->m[0][2]*in1->m[2][3] + in1->m[3][1]*in1->m[0][3]*in1->m[2][2];
210         temp.m[1][1] =  in1->m[0][0]*in1->m[2][2]*in1->m[3][3] - in1->m[0][0]*in1->m[2][3]*in1->m[3][2] - in1->m[2][0]*in1->m[0][2]*in1->m[3][3] + in1->m[2][0]*in1->m[0][3]*in1->m[3][2] + in1->m[3][0]*in1->m[0][2]*in1->m[2][3] - in1->m[3][0]*in1->m[0][3]*in1->m[2][2];
211         temp.m[2][1] = -in1->m[0][0]*in1->m[2][1]*in1->m[3][3] + in1->m[0][0]*in1->m[2][3]*in1->m[3][1] + in1->m[2][0]*in1->m[0][1]*in1->m[3][3] - in1->m[2][0]*in1->m[0][3]*in1->m[3][1] - in1->m[3][0]*in1->m[0][1]*in1->m[2][3] + in1->m[3][0]*in1->m[0][3]*in1->m[2][1];
212         temp.m[3][1] =  in1->m[0][0]*in1->m[2][1]*in1->m[3][2] - in1->m[0][0]*in1->m[2][2]*in1->m[3][1] - in1->m[2][0]*in1->m[0][1]*in1->m[3][2] + in1->m[2][0]*in1->m[0][2]*in1->m[3][1] + in1->m[3][0]*in1->m[0][1]*in1->m[2][2] - in1->m[3][0]*in1->m[0][2]*in1->m[2][1];
213         temp.m[0][2] =  in1->m[0][1]*in1->m[1][2]*in1->m[3][3] - in1->m[0][1]*in1->m[1][3]*in1->m[3][2] - in1->m[1][1]*in1->m[0][2]*in1->m[3][3] + in1->m[1][1]*in1->m[0][3]*in1->m[3][2] + in1->m[3][1]*in1->m[0][2]*in1->m[1][3] - in1->m[3][1]*in1->m[0][3]*in1->m[1][2];
214         temp.m[1][2] = -in1->m[0][0]*in1->m[1][2]*in1->m[3][3] + in1->m[0][0]*in1->m[1][3]*in1->m[3][2] + in1->m[1][0]*in1->m[0][2]*in1->m[3][3] - in1->m[1][0]*in1->m[0][3]*in1->m[3][2] - in1->m[3][0]*in1->m[0][2]*in1->m[1][3] + in1->m[3][0]*in1->m[0][3]*in1->m[1][2];
215         temp.m[2][2] =  in1->m[0][0]*in1->m[1][1]*in1->m[3][3] - in1->m[0][0]*in1->m[1][3]*in1->m[3][1] - in1->m[1][0]*in1->m[0][1]*in1->m[3][3] + in1->m[1][0]*in1->m[0][3]*in1->m[3][1] + in1->m[3][0]*in1->m[0][1]*in1->m[1][3] - in1->m[3][0]*in1->m[0][3]*in1->m[1][1];
216         temp.m[3][2] = -in1->m[0][0]*in1->m[1][1]*in1->m[3][2] + in1->m[0][0]*in1->m[1][2]*in1->m[3][1] + in1->m[1][0]*in1->m[0][1]*in1->m[3][2] - in1->m[1][0]*in1->m[0][2]*in1->m[3][1] - in1->m[3][0]*in1->m[0][1]*in1->m[1][2] + in1->m[3][0]*in1->m[0][2]*in1->m[1][1];
217         temp.m[0][3] = -in1->m[0][1]*in1->m[1][2]*in1->m[2][3] + in1->m[0][1]*in1->m[1][3]*in1->m[2][2] + in1->m[1][1]*in1->m[0][2]*in1->m[2][3] - in1->m[1][1]*in1->m[0][3]*in1->m[2][2] - in1->m[2][1]*in1->m[0][2]*in1->m[1][3] + in1->m[2][1]*in1->m[0][3]*in1->m[1][2];
218         temp.m[1][3] =  in1->m[0][0]*in1->m[1][2]*in1->m[2][3] - in1->m[0][0]*in1->m[1][3]*in1->m[2][2] - in1->m[1][0]*in1->m[0][2]*in1->m[2][3] + in1->m[1][0]*in1->m[0][3]*in1->m[2][2] + in1->m[2][0]*in1->m[0][2]*in1->m[1][3] - in1->m[2][0]*in1->m[0][3]*in1->m[1][2];
219         temp.m[2][3] = -in1->m[0][0]*in1->m[1][1]*in1->m[2][3] + in1->m[0][0]*in1->m[1][3]*in1->m[2][1] + in1->m[1][0]*in1->m[0][1]*in1->m[2][3] - in1->m[1][0]*in1->m[0][3]*in1->m[2][1] - in1->m[2][0]*in1->m[0][1]*in1->m[1][3] + in1->m[2][0]*in1->m[0][3]*in1->m[1][1];
220         temp.m[3][3] =  in1->m[0][0]*in1->m[1][1]*in1->m[2][2] - in1->m[0][0]*in1->m[1][2]*in1->m[2][1] - in1->m[1][0]*in1->m[0][1]*in1->m[2][2] + in1->m[1][0]*in1->m[0][2]*in1->m[2][1] + in1->m[2][0]*in1->m[0][1]*in1->m[1][2] - in1->m[2][0]*in1->m[0][2]*in1->m[1][1];
221 #else
222         temp.m[0][0] =  in1->m[1][1]*in1->m[2][2]*in1->m[3][3] - in1->m[1][1]*in1->m[3][2]*in1->m[2][3] - in1->m[1][2]*in1->m[2][1]*in1->m[3][3] + in1->m[1][2]*in1->m[3][1]*in1->m[2][3] + in1->m[1][3]*in1->m[2][1]*in1->m[3][2] - in1->m[1][3]*in1->m[3][1]*in1->m[2][2];
223         temp.m[0][1] = -in1->m[0][1]*in1->m[2][2]*in1->m[3][3] + in1->m[0][1]*in1->m[3][2]*in1->m[2][3] + in1->m[0][2]*in1->m[2][1]*in1->m[3][3] - in1->m[0][2]*in1->m[3][1]*in1->m[2][3] - in1->m[0][3]*in1->m[2][1]*in1->m[3][2] + in1->m[0][3]*in1->m[3][1]*in1->m[2][2];
224         temp.m[0][2] =  in1->m[0][1]*in1->m[1][2]*in1->m[3][3] - in1->m[0][1]*in1->m[3][2]*in1->m[1][3] - in1->m[0][2]*in1->m[1][1]*in1->m[3][3] + in1->m[0][2]*in1->m[3][1]*in1->m[1][3] + in1->m[0][3]*in1->m[1][1]*in1->m[3][2] - in1->m[0][3]*in1->m[3][1]*in1->m[1][2];
225         temp.m[0][3] = -in1->m[0][1]*in1->m[1][2]*in1->m[2][3] + in1->m[0][1]*in1->m[2][2]*in1->m[1][3] + in1->m[0][2]*in1->m[1][1]*in1->m[2][3] - in1->m[0][2]*in1->m[2][1]*in1->m[1][3] - in1->m[0][3]*in1->m[1][1]*in1->m[2][2] + in1->m[0][3]*in1->m[2][1]*in1->m[1][2];
226         temp.m[1][0] = -in1->m[1][0]*in1->m[2][2]*in1->m[3][3] + in1->m[1][0]*in1->m[3][2]*in1->m[2][3] + in1->m[1][2]*in1->m[2][0]*in1->m[3][3] - in1->m[1][2]*in1->m[3][0]*in1->m[2][3] - in1->m[1][3]*in1->m[2][0]*in1->m[3][2] + in1->m[1][3]*in1->m[3][0]*in1->m[2][2];
227         temp.m[1][1] =  in1->m[0][0]*in1->m[2][2]*in1->m[3][3] - in1->m[0][0]*in1->m[3][2]*in1->m[2][3] - in1->m[0][2]*in1->m[2][0]*in1->m[3][3] + in1->m[0][2]*in1->m[3][0]*in1->m[2][3] + in1->m[0][3]*in1->m[2][0]*in1->m[3][2] - in1->m[0][3]*in1->m[3][0]*in1->m[2][2];
228         temp.m[1][2] = -in1->m[0][0]*in1->m[1][2]*in1->m[3][3] + in1->m[0][0]*in1->m[3][2]*in1->m[1][3] + in1->m[0][2]*in1->m[1][0]*in1->m[3][3] - in1->m[0][2]*in1->m[3][0]*in1->m[1][3] - in1->m[0][3]*in1->m[1][0]*in1->m[3][2] + in1->m[0][3]*in1->m[3][0]*in1->m[1][2];
229         temp.m[1][3] =  in1->m[0][0]*in1->m[1][2]*in1->m[2][3] - in1->m[0][0]*in1->m[2][2]*in1->m[1][3] - in1->m[0][2]*in1->m[1][0]*in1->m[2][3] + in1->m[0][2]*in1->m[2][0]*in1->m[1][3] + in1->m[0][3]*in1->m[1][0]*in1->m[2][2] - in1->m[0][3]*in1->m[2][0]*in1->m[1][2];
230         temp.m[2][0] =  in1->m[1][0]*in1->m[2][1]*in1->m[3][3] - in1->m[1][0]*in1->m[3][1]*in1->m[2][3] - in1->m[1][1]*in1->m[2][0]*in1->m[3][3] + in1->m[1][1]*in1->m[3][0]*in1->m[2][3] + in1->m[1][3]*in1->m[2][0]*in1->m[3][1] - in1->m[1][3]*in1->m[3][0]*in1->m[2][1];
231         temp.m[2][1] = -in1->m[0][0]*in1->m[2][1]*in1->m[3][3] + in1->m[0][0]*in1->m[3][1]*in1->m[2][3] + in1->m[0][1]*in1->m[2][0]*in1->m[3][3] - in1->m[0][1]*in1->m[3][0]*in1->m[2][3] - in1->m[0][3]*in1->m[2][0]*in1->m[3][1] + in1->m[0][3]*in1->m[3][0]*in1->m[2][1];
232         temp.m[2][2] =  in1->m[0][0]*in1->m[1][1]*in1->m[3][3] - in1->m[0][0]*in1->m[3][1]*in1->m[1][3] - in1->m[0][1]*in1->m[1][0]*in1->m[3][3] + in1->m[0][1]*in1->m[3][0]*in1->m[1][3] + in1->m[0][3]*in1->m[1][0]*in1->m[3][1] - in1->m[0][3]*in1->m[3][0]*in1->m[1][1];
233         temp.m[2][3] = -in1->m[0][0]*in1->m[1][1]*in1->m[2][3] + in1->m[0][0]*in1->m[2][1]*in1->m[1][3] + in1->m[0][1]*in1->m[1][0]*in1->m[2][3] - in1->m[0][1]*in1->m[2][0]*in1->m[1][3] - in1->m[0][3]*in1->m[1][0]*in1->m[2][1] + in1->m[0][3]*in1->m[2][0]*in1->m[1][1];
234         temp.m[3][0] = -in1->m[1][0]*in1->m[2][1]*in1->m[3][2] + in1->m[1][0]*in1->m[3][1]*in1->m[2][2] + in1->m[1][1]*in1->m[2][0]*in1->m[3][2] - in1->m[1][1]*in1->m[3][0]*in1->m[2][2] - in1->m[1][2]*in1->m[2][0]*in1->m[3][1] + in1->m[1][2]*in1->m[3][0]*in1->m[2][1];
235         temp.m[3][1] =  in1->m[0][0]*in1->m[2][1]*in1->m[3][2] - in1->m[0][0]*in1->m[3][1]*in1->m[2][2] - in1->m[0][1]*in1->m[2][0]*in1->m[3][2] + in1->m[0][1]*in1->m[3][0]*in1->m[2][2] + in1->m[0][2]*in1->m[2][0]*in1->m[3][1] - in1->m[0][2]*in1->m[3][0]*in1->m[2][1];
236         temp.m[3][2] = -in1->m[0][0]*in1->m[1][1]*in1->m[3][2] + in1->m[0][0]*in1->m[3][1]*in1->m[1][2] + in1->m[0][1]*in1->m[1][0]*in1->m[3][2] - in1->m[0][1]*in1->m[3][0]*in1->m[1][2] - in1->m[0][2]*in1->m[1][0]*in1->m[3][1] + in1->m[0][2]*in1->m[3][0]*in1->m[1][1];
237         temp.m[3][3] =  in1->m[0][0]*in1->m[1][1]*in1->m[2][2] - in1->m[0][0]*in1->m[2][1]*in1->m[1][2] - in1->m[0][1]*in1->m[1][0]*in1->m[2][2] + in1->m[0][1]*in1->m[2][0]*in1->m[1][2] + in1->m[0][2]*in1->m[1][0]*in1->m[2][1] - in1->m[0][2]*in1->m[2][0]*in1->m[1][1];
238 #endif
239
240         det = in1->m[0][0]*temp.m[0][0] + in1->m[1][0]*temp.m[0][1] + in1->m[2][0]*temp.m[0][2] + in1->m[3][0]*temp.m[0][3];
241         if (det == 0.0f)
242                 return 0;
243
244         det = 1.0f / det;
245
246         for (i = 0;i < 4;i++)
247                 for (j = 0;j < 4;j++)
248                         out->m[i][j] = temp.m[i][j] * det;
249
250         return 1;
251 }
252 #else
253 int Matrix4x4_Invert_Full (matrix4x4_t *out, const matrix4x4_t *in1)
254 {
255         double  *temp;
256         double  *r[4];
257         double  rtemp[4][8];
258         double  m[4];
259         double  s;
260
261         r[0]    = rtemp[0];
262         r[1]    = rtemp[1];
263         r[2]    = rtemp[2];
264         r[3]    = rtemp[3];
265
266 #ifdef MATRIX4x4_OPENGLORIENTATION
267         r[0][0] = in1->m[0][0]; r[0][1] = in1->m[1][0]; r[0][2] = in1->m[2][0]; r[0][3] = in1->m[3][0];
268         r[0][4] = 1.0;                  r[0][5] =                               r[0][6] =                               r[0][7] = 0.0;
269
270         r[1][0] = in1->m[0][1]; r[1][1] = in1->m[1][1]; r[1][2] = in1->m[2][1]; r[1][3] = in1->m[3][1];
271         r[1][5] = 1.0;                  r[1][4] =                               r[1][6] =                               r[1][7] = 0.0;
272
273         r[2][0] = in1->m[0][2]; r[2][1] = in1->m[1][2]; r[2][2] = in1->m[2][2]; r[2][3] = in1->m[3][2];
274         r[2][6] = 1.0;                  r[2][4] =                               r[2][5] =                               r[2][7] = 0.0;
275
276         r[3][0] = in1->m[0][3]; r[3][1] = in1->m[1][3]; r[3][2] = in1->m[2][3]; r[3][3] = in1->m[3][3];
277         r[3][7] = 1.0;                  r[3][4] =                               r[3][5] =                               r[3][6] = 0.0;
278 #else
279         r[0][0] = in1->m[0][0]; r[0][1] = in1->m[0][1]; r[0][2] = in1->m[0][2]; r[0][3] = in1->m[0][3];
280         r[0][4] = 1.0;                  r[0][5] =                               r[0][6] =                               r[0][7] = 0.0;
281
282         r[1][0] = in1->m[1][0]; r[1][1] = in1->m[1][1]; r[1][2] = in1->m[1][2]; r[1][3] = in1->m[1][3];
283         r[1][5] = 1.0;                  r[1][4] =                               r[1][6] =                               r[1][7] = 0.0;
284
285         r[2][0] = in1->m[2][0]; r[2][1] = in1->m[2][1]; r[2][2] = in1->m[2][2]; r[2][3] = in1->m[2][3];
286         r[2][6] = 1.0;                  r[2][4] =                               r[2][5] =                               r[2][7] = 0.0;
287
288         r[3][0] = in1->m[3][0]; r[3][1] = in1->m[3][1]; r[3][2] = in1->m[3][2]; r[3][3] = in1->m[3][3];
289         r[3][7] = 1.0;                  r[3][4] =                               r[3][5] =                               r[3][6] = 0.0;
290 #endif
291
292         if (fabs (r[3][0]) > fabs (r[2][0])) { temp = r[3]; r[3] = r[2]; r[2] = temp; }
293         if (fabs (r[2][0]) > fabs (r[1][0])) { temp = r[2]; r[2] = r[1]; r[1] = temp; }
294         if (fabs (r[1][0]) > fabs (r[0][0])) { temp = r[1]; r[1] = r[0]; r[0] = temp; }
295
296         if (r[0][0])
297         {
298                 m[1]    = r[1][0] / r[0][0];
299                 m[2]    = r[2][0] / r[0][0];
300                 m[3]    = r[3][0] / r[0][0];
301
302                 s       = r[0][1]; r[1][1] -= m[1] * s; r[2][1] -= m[2] * s; r[3][1] -= m[3] * s;
303                 s       = r[0][2]; r[1][2] -= m[1] * s; r[2][2] -= m[2] * s; r[3][2] -= m[3] * s;
304                 s       = r[0][3]; r[1][3] -= m[1] * s; r[2][3] -= m[2] * s; r[3][3] -= m[3] * s;
305
306                 s       = r[0][4]; if (s) { r[1][4] -= m[1] * s; r[2][4] -= m[2] * s; r[3][4] -= m[3] * s; }
307                 s       = r[0][5]; if (s) { r[1][5] -= m[1] * s; r[2][5] -= m[2] * s; r[3][5] -= m[3] * s; }
308                 s       = r[0][6]; if (s) { r[1][6] -= m[1] * s; r[2][6] -= m[2] * s; r[3][6] -= m[3] * s; }
309                 s       = r[0][7]; if (s) { r[1][7] -= m[1] * s; r[2][7] -= m[2] * s; r[3][7] -= m[3] * s; }
310
311                 if (fabs (r[3][1]) > fabs (r[2][1])) { temp = r[3]; r[3] = r[2]; r[2] = temp; }
312                 if (fabs (r[2][1]) > fabs (r[1][1])) { temp = r[2]; r[2] = r[1]; r[1] = temp; }
313
314                 if (r[1][1])
315                 {
316                         m[2]            = r[2][1] / r[1][1];
317                         m[3]            = r[3][1] / r[1][1];
318                         r[2][2] -= m[2] * r[1][2];
319                         r[3][2] -= m[3] * r[1][2];
320                         r[2][3] -= m[2] * r[1][3];
321                         r[3][3] -= m[3] * r[1][3];
322
323                         s       = r[1][4]; if (s) { r[2][4] -= m[2] * s; r[3][4] -= m[3] * s; }
324                         s       = r[1][5]; if (s) { r[2][5] -= m[2] * s; r[3][5] -= m[3] * s; }
325                         s       = r[1][6]; if (s) { r[2][6] -= m[2] * s; r[3][6] -= m[3] * s; }
326                         s       = r[1][7]; if (s) { r[2][7] -= m[2] * s; r[3][7] -= m[3] * s; }
327
328                         if (fabs (r[3][2]) > fabs (r[2][2])) { temp = r[3]; r[3] = r[2]; r[2] = temp; }
329
330                         if (r[2][2])
331                         {
332                                 m[3]            = r[3][2] / r[2][2];
333                                 r[3][3] -= m[3] * r[2][3];
334                                 r[3][4] -= m[3] * r[2][4];
335                                 r[3][5] -= m[3] * r[2][5];
336                                 r[3][6] -= m[3] * r[2][6];
337                                 r[3][7] -= m[3] * r[2][7];
338
339                                 if (r[3][3])
340                                 {
341                                         s                       = 1.0 / r[3][3];
342                                         r[3][4] *= s;
343                                         r[3][5] *= s;
344                                         r[3][6] *= s;
345                                         r[3][7] *= s;
346
347                                         m[2]            = r[2][3];
348                                         s                       = 1.0 / r[2][2];
349                                         r[2][4] = s * (r[2][4] - r[3][4] * m[2]);
350                                         r[2][5] = s * (r[2][5] - r[3][5] * m[2]);
351                                         r[2][6] = s * (r[2][6] - r[3][6] * m[2]);
352                                         r[2][7] = s * (r[2][7] - r[3][7] * m[2]);
353
354                                         m[1]            = r[1][3];
355                                         r[1][4] -= r[3][4] * m[1], r[1][5] -= r[3][5] * m[1];
356                                         r[1][6] -= r[3][6] * m[1], r[1][7] -= r[3][7] * m[1];
357
358                                         m[0]            = r[0][3];
359                                         r[0][4] -= r[3][4] * m[0], r[0][5] -= r[3][5] * m[0];
360                                         r[0][6] -= r[3][6] * m[0], r[0][7] -= r[3][7] * m[0];
361
362                                         m[1]            = r[1][2];
363                                         s                       = 1.0 / r[1][1];
364                                         r[1][4] = s * (r[1][4] - r[2][4] * m[1]), r[1][5] = s * (r[1][5] - r[2][5] * m[1]);
365                                         r[1][6] = s * (r[1][6] - r[2][6] * m[1]), r[1][7] = s * (r[1][7] - r[2][7] * m[1]);
366
367                                         m[0]            = r[0][2];
368                                         r[0][4] -= r[2][4] * m[0], r[0][5] -= r[2][5] * m[0];
369                                         r[0][6] -= r[2][6] * m[0], r[0][7] -= r[2][7] * m[0];
370
371                                         m[0]            = r[0][1];
372                                         s                       = 1.0 / r[0][0];
373                                         r[0][4] = s * (r[0][4] - r[1][4] * m[0]), r[0][5] = s * (r[0][5] - r[1][5] * m[0]);
374                                         r[0][6] = s * (r[0][6] - r[1][6] * m[0]), r[0][7] = s * (r[0][7] - r[1][7] * m[0]);
375
376 #ifdef MATRIX4x4_OPENGLORIENTATION
377                                         out->m[0][0]    = r[0][4];
378                                         out->m[0][1]    = r[1][4];
379                                         out->m[0][2]    = r[2][4];
380                                         out->m[0][3]    = r[3][4];
381                                         out->m[1][0]    = r[0][5];
382                                         out->m[1][1]    = r[1][5];
383                                         out->m[1][2]    = r[2][5];
384                                         out->m[1][3]    = r[3][5];
385                                         out->m[2][0]    = r[0][6];
386                                         out->m[2][1]    = r[1][6];
387                                         out->m[2][2]    = r[2][6];
388                                         out->m[2][3]    = r[3][6];
389                                         out->m[3][0]    = r[0][7];
390                                         out->m[3][1]    = r[1][7];
391                                         out->m[3][2]    = r[2][7];
392                                         out->m[3][3]    = r[3][7];
393 #else
394                                         out->m[0][0]    = r[0][4];
395                                         out->m[0][1]    = r[0][5];
396                                         out->m[0][2]    = r[0][6];
397                                         out->m[0][3]    = r[0][7];
398                                         out->m[1][0]    = r[1][4];
399                                         out->m[1][1]    = r[1][5];
400                                         out->m[1][2]    = r[1][6];
401                                         out->m[1][3]    = r[1][7];
402                                         out->m[2][0]    = r[2][4];
403                                         out->m[2][1]    = r[2][5];
404                                         out->m[2][2]    = r[2][6];
405                                         out->m[2][3]    = r[2][7];
406                                         out->m[3][0]    = r[3][4];
407                                         out->m[3][1]    = r[3][5];
408                                         out->m[3][2]    = r[3][6];
409                                         out->m[3][3]    = r[3][7];
410 #endif
411
412                                         return 1;
413                                 }
414                         }
415                 }
416         }
417
418         return 0;
419 }
420 #endif
421
422 void Matrix4x4_Invert_Simple (matrix4x4_t *out, const matrix4x4_t *in1)
423 {
424         // we only support uniform scaling, so assume the first row is enough
425         // (note the lack of sqrt here, because we're trying to undo the scaling,
426         // this means multiplying by the inverse scale twice - squaring it, which
427         // makes the sqrt a waste of time)
428 #if 1
429         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]);
430 #else
431         double scale = 3.0 / sqrt
432                  (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]
433                 + in1->m[1][0] * in1->m[1][0] + in1->m[1][1] * in1->m[1][1] + in1->m[1][2] * in1->m[1][2]
434                 + in1->m[2][0] * in1->m[2][0] + in1->m[2][1] * in1->m[2][1] + in1->m[2][2] * in1->m[2][2]);
435         scale *= scale;
436 #endif
437
438         // invert the rotation by transposing and multiplying by the squared
439         // recipricol of the input matrix scale as described above
440         out->m[0][0] = in1->m[0][0] * scale;
441         out->m[0][1] = in1->m[1][0] * scale;
442         out->m[0][2] = in1->m[2][0] * scale;
443         out->m[1][0] = in1->m[0][1] * scale;
444         out->m[1][1] = in1->m[1][1] * scale;
445         out->m[1][2] = in1->m[2][1] * scale;
446         out->m[2][0] = in1->m[0][2] * scale;
447         out->m[2][1] = in1->m[1][2] * scale;
448         out->m[2][2] = in1->m[2][2] * scale;
449
450 #ifdef MATRIX4x4_OPENGLORIENTATION
451         // invert the translate
452         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]);
453         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]);
454         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]);
455
456         // don't know if there's anything worth doing here
457         out->m[0][3] = 0;
458         out->m[1][3] = 0;
459         out->m[2][3] = 0;
460         out->m[3][3] = 1;
461 #else
462         // invert the translate
463         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]);
464         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]);
465         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]);
466
467         // don't know if there's anything worth doing here
468         out->m[3][0] = 0;
469         out->m[3][1] = 0;
470         out->m[3][2] = 0;
471         out->m[3][3] = 1;
472 #endif
473 }
474
475 void Matrix4x4_Interpolate (matrix4x4_t *out, matrix4x4_t *in1, matrix4x4_t *in2, double frac)
476 {
477         int i, j;
478         for (i = 0;i < 4;i++)
479                 for (j = 0;j < 4;j++)
480                         out->m[i][j] = in1->m[i][j] + frac * (in2->m[i][j] - in1->m[i][j]);
481 }
482
483 void Matrix4x4_Clear (matrix4x4_t *out)
484 {
485         int i, j;
486         for (i = 0;i < 4;i++)
487                 for (j = 0;j < 4;j++)
488                         out->m[i][j] = 0;
489 }
490
491 void Matrix4x4_Accumulate (matrix4x4_t *out, matrix4x4_t *in, double weight)
492 {
493         int i, j;
494         for (i = 0;i < 4;i++)
495                 for (j = 0;j < 4;j++)
496                         out->m[i][j] += in->m[i][j] * weight;
497 }
498
499 void Matrix4x4_Normalize (matrix4x4_t *out, matrix4x4_t *in1)
500 {
501         // scale rotation matrix vectors to a length of 1
502         // note: this is only designed to undo uniform scaling
503         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]);
504         *out = *in1;
505         Matrix4x4_Scale(out, scale, 1);
506 }
507
508 void Matrix4x4_Normalize3 (matrix4x4_t *out, matrix4x4_t *in1)
509 {
510         int i;
511         double scale;
512         // scale each rotation matrix vector to a length of 1
513         // intended for use after Matrix4x4_Interpolate or Matrix4x4_Accumulate
514         *out = *in1;
515         for (i = 0;i < 3;i++)
516         {
517 #ifdef MATRIX4x4_OPENGLORIENTATION
518                 scale = sqrt(in1->m[i][0] * in1->m[i][0] + in1->m[i][1] * in1->m[i][1] + in1->m[i][2] * in1->m[i][2]);
519                 if (scale)
520                         scale = 1.0 / scale;
521                 out->m[i][0] *= scale;
522                 out->m[i][1] *= scale;
523                 out->m[i][2] *= scale;
524 #else
525                 scale = sqrt(in1->m[0][i] * in1->m[0][i] + in1->m[1][i] * in1->m[1][i] + in1->m[2][i] * in1->m[2][i]);
526                 if (scale)
527                         scale = 1.0 / scale;
528                 out->m[0][i] *= scale;
529                 out->m[1][i] *= scale;
530                 out->m[2][i] *= scale;
531 #endif
532         }
533 }
534
535 void Matrix4x4_Reflect (matrix4x4_t *out, double normalx, double normaly, double normalz, double dist, double axisscale)
536 {
537         int i;
538         double d;
539         double p[4], p2[4];
540         p[0] = normalx;
541         p[1] = normaly;
542         p[2] = normalz;
543         p[3] = -dist;
544         p2[0] = p[0] * axisscale;
545         p2[1] = p[1] * axisscale;
546         p2[2] = p[2] * axisscale;
547         p2[3] = 0;
548         for (i = 0;i < 4;i++)
549         {
550 #ifdef MATRIX4x4_OPENGLORIENTATION
551                 d = out->m[i][0] * p[0] + out->m[i][1] * p[1] + out->m[i][2] * p[2] + out->m[i][3] * p[3];
552                 out->m[i][0] += p2[0] * d;
553                 out->m[i][1] += p2[1] * d;
554                 out->m[i][2] += p2[2] * d;
555 #else
556                 d = out->m[0][i] * p[0] + out->m[1][i] * p[1] + out->m[2][i] * p[2] + out->m[3][i] * p[3];
557                 out->m[0][i] += p2[0] * d;
558                 out->m[1][i] += p2[1] * d;
559                 out->m[2][i] += p2[2] * d;
560 #endif
561         }
562 }
563
564 void Matrix4x4_CreateIdentity (matrix4x4_t *out)
565 {
566         out->m[0][0]=1.0f;
567         out->m[0][1]=0.0f;
568         out->m[0][2]=0.0f;
569         out->m[0][3]=0.0f;
570         out->m[1][0]=0.0f;
571         out->m[1][1]=1.0f;
572         out->m[1][2]=0.0f;
573         out->m[1][3]=0.0f;
574         out->m[2][0]=0.0f;
575         out->m[2][1]=0.0f;
576         out->m[2][2]=1.0f;
577         out->m[2][3]=0.0f;
578         out->m[3][0]=0.0f;
579         out->m[3][1]=0.0f;
580         out->m[3][2]=0.0f;
581         out->m[3][3]=1.0f;
582 }
583
584 void Matrix4x4_CreateTranslate (matrix4x4_t *out, double x, double y, double z)
585 {
586 #ifdef MATRIX4x4_OPENGLORIENTATION
587         out->m[0][0]=1.0f;
588         out->m[1][0]=0.0f;
589         out->m[2][0]=0.0f;
590         out->m[3][0]=x;
591         out->m[0][1]=0.0f;
592         out->m[1][1]=1.0f;
593         out->m[2][1]=0.0f;
594         out->m[3][1]=y;
595         out->m[0][2]=0.0f;
596         out->m[1][2]=0.0f;
597         out->m[2][2]=1.0f;
598         out->m[3][2]=z;
599         out->m[0][3]=0.0f;
600         out->m[1][3]=0.0f;
601         out->m[2][3]=0.0f;
602         out->m[3][3]=1.0f;
603 #else
604         out->m[0][0]=1.0f;
605         out->m[0][1]=0.0f;
606         out->m[0][2]=0.0f;
607         out->m[0][3]=x;
608         out->m[1][0]=0.0f;
609         out->m[1][1]=1.0f;
610         out->m[1][2]=0.0f;
611         out->m[1][3]=y;
612         out->m[2][0]=0.0f;
613         out->m[2][1]=0.0f;
614         out->m[2][2]=1.0f;
615         out->m[2][3]=z;
616         out->m[3][0]=0.0f;
617         out->m[3][1]=0.0f;
618         out->m[3][2]=0.0f;
619         out->m[3][3]=1.0f;
620 #endif
621 }
622
623 void Matrix4x4_CreateRotate (matrix4x4_t *out, double angle, double x, double y, double z)
624 {
625         double len, c, s;
626
627         len = x*x+y*y+z*z;
628         if (len != 0.0f)
629                 len = 1.0f / sqrt(len);
630         x *= len;
631         y *= len;
632         z *= len;
633
634         angle *= (-M_PI / 180.0);
635         c = cos(angle);
636         s = sin(angle);
637
638 #ifdef MATRIX4x4_OPENGLORIENTATION
639         out->m[0][0]=x * x + c * (1 - x * x);
640         out->m[1][0]=x * y * (1 - c) + z * s;
641         out->m[2][0]=z * x * (1 - c) - y * s;
642         out->m[3][0]=0.0f;
643         out->m[0][1]=x * y * (1 - c) - z * s;
644         out->m[1][1]=y * y + c * (1 - y * y);
645         out->m[2][1]=y * z * (1 - c) + x * s;
646         out->m[3][1]=0.0f;
647         out->m[0][2]=z * x * (1 - c) + y * s;
648         out->m[1][2]=y * z * (1 - c) - x * s;
649         out->m[2][2]=z * z + c * (1 - z * z);
650         out->m[3][2]=0.0f;
651         out->m[0][3]=0.0f;
652         out->m[1][3]=0.0f;
653         out->m[2][3]=0.0f;
654         out->m[3][3]=1.0f;
655 #else
656         out->m[0][0]=x * x + c * (1 - x * x);
657         out->m[0][1]=x * y * (1 - c) + z * s;
658         out->m[0][2]=z * x * (1 - c) - y * s;
659         out->m[0][3]=0.0f;
660         out->m[1][0]=x * y * (1 - c) - z * s;
661         out->m[1][1]=y * y + c * (1 - y * y);
662         out->m[1][2]=y * z * (1 - c) + x * s;
663         out->m[1][3]=0.0f;
664         out->m[2][0]=z * x * (1 - c) + y * s;
665         out->m[2][1]=y * z * (1 - c) - x * s;
666         out->m[2][2]=z * z + c * (1 - z * z);
667         out->m[2][3]=0.0f;
668         out->m[3][0]=0.0f;
669         out->m[3][1]=0.0f;
670         out->m[3][2]=0.0f;
671         out->m[3][3]=1.0f;
672 #endif
673 }
674
675 void Matrix4x4_CreateScale (matrix4x4_t *out, double x)
676 {
677         out->m[0][0]=x;
678         out->m[0][1]=0.0f;
679         out->m[0][2]=0.0f;
680         out->m[0][3]=0.0f;
681         out->m[1][0]=0.0f;
682         out->m[1][1]=x;
683         out->m[1][2]=0.0f;
684         out->m[1][3]=0.0f;
685         out->m[2][0]=0.0f;
686         out->m[2][1]=0.0f;
687         out->m[2][2]=x;
688         out->m[2][3]=0.0f;
689         out->m[3][0]=0.0f;
690         out->m[3][1]=0.0f;
691         out->m[3][2]=0.0f;
692         out->m[3][3]=1.0f;
693 }
694
695 void Matrix4x4_CreateScale3 (matrix4x4_t *out, double x, double y, double z)
696 {
697         out->m[0][0]=x;
698         out->m[0][1]=0.0f;
699         out->m[0][2]=0.0f;
700         out->m[0][3]=0.0f;
701         out->m[1][0]=0.0f;
702         out->m[1][1]=y;
703         out->m[1][2]=0.0f;
704         out->m[1][3]=0.0f;
705         out->m[2][0]=0.0f;
706         out->m[2][1]=0.0f;
707         out->m[2][2]=z;
708         out->m[2][3]=0.0f;
709         out->m[3][0]=0.0f;
710         out->m[3][1]=0.0f;
711         out->m[3][2]=0.0f;
712         out->m[3][3]=1.0f;
713 }
714
715 void Matrix4x4_CreateFromQuakeEntity(matrix4x4_t *out, double x, double y, double z, double pitch, double yaw, double roll, double scale)
716 {
717         double angle, sr, sp, sy, cr, cp, cy;
718
719         if (roll)
720         {
721                 angle = yaw * (M_PI*2 / 360);
722                 sy = sin(angle);
723                 cy = cos(angle);
724                 angle = pitch * (M_PI*2 / 360);
725                 sp = sin(angle);
726                 cp = cos(angle);
727                 angle = roll * (M_PI*2 / 360);
728                 sr = sin(angle);
729                 cr = cos(angle);
730 #ifdef MATRIX4x4_OPENGLORIENTATION
731                 out->m[0][0] = (cp*cy) * scale;
732                 out->m[1][0] = (sr*sp*cy+cr*-sy) * scale;
733                 out->m[2][0] = (cr*sp*cy+-sr*-sy) * scale;
734                 out->m[3][0] = x;
735                 out->m[0][1] = (cp*sy) * scale;
736                 out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
737                 out->m[2][1] = (cr*sp*sy+-sr*cy) * scale;
738                 out->m[3][1] = y;
739                 out->m[0][2] = (-sp) * scale;
740                 out->m[1][2] = (sr*cp) * scale;
741                 out->m[2][2] = (cr*cp) * scale;
742                 out->m[3][2] = z;
743                 out->m[0][3] = 0;
744                 out->m[1][3] = 0;
745                 out->m[2][3] = 0;
746                 out->m[3][3] = 1;
747 #else
748                 out->m[0][0] = (cp*cy) * scale;
749                 out->m[0][1] = (sr*sp*cy+cr*-sy) * scale;
750                 out->m[0][2] = (cr*sp*cy+-sr*-sy) * scale;
751                 out->m[0][3] = x;
752                 out->m[1][0] = (cp*sy) * scale;
753                 out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
754                 out->m[1][2] = (cr*sp*sy+-sr*cy) * scale;
755                 out->m[1][3] = y;
756                 out->m[2][0] = (-sp) * scale;
757                 out->m[2][1] = (sr*cp) * scale;
758                 out->m[2][2] = (cr*cp) * scale;
759                 out->m[2][3] = z;
760                 out->m[3][0] = 0;
761                 out->m[3][1] = 0;
762                 out->m[3][2] = 0;
763                 out->m[3][3] = 1;
764 #endif
765         }
766         else if (pitch)
767         {
768                 angle = yaw * (M_PI*2 / 360);
769                 sy = sin(angle);
770                 cy = cos(angle);
771                 angle = pitch * (M_PI*2 / 360);
772                 sp = sin(angle);
773                 cp = cos(angle);
774 #ifdef MATRIX4x4_OPENGLORIENTATION
775                 out->m[0][0] = (cp*cy) * scale;
776                 out->m[1][0] = (-sy) * scale;
777                 out->m[2][0] = (sp*cy) * scale;
778                 out->m[3][0] = x;
779                 out->m[0][1] = (cp*sy) * scale;
780                 out->m[1][1] = (cy) * scale;
781                 out->m[2][1] = (sp*sy) * scale;
782                 out->m[3][1] = y;
783                 out->m[0][2] = (-sp) * scale;
784                 out->m[1][2] = 0;
785                 out->m[2][2] = (cp) * scale;
786                 out->m[3][2] = z;
787                 out->m[0][3] = 0;
788                 out->m[1][3] = 0;
789                 out->m[2][3] = 0;
790                 out->m[3][3] = 1;
791 #else
792                 out->m[0][0] = (cp*cy) * scale;
793                 out->m[0][1] = (-sy) * scale;
794                 out->m[0][2] = (sp*cy) * scale;
795                 out->m[0][3] = x;
796                 out->m[1][0] = (cp*sy) * scale;
797                 out->m[1][1] = (cy) * scale;
798                 out->m[1][2] = (sp*sy) * scale;
799                 out->m[1][3] = y;
800                 out->m[2][0] = (-sp) * scale;
801                 out->m[2][1] = 0;
802                 out->m[2][2] = (cp) * scale;
803                 out->m[2][3] = z;
804                 out->m[3][0] = 0;
805                 out->m[3][1] = 0;
806                 out->m[3][2] = 0;
807                 out->m[3][3] = 1;
808 #endif
809         }
810         else if (yaw)
811         {
812                 angle = yaw * (M_PI*2 / 360);
813                 sy = sin(angle);
814                 cy = cos(angle);
815 #ifdef MATRIX4x4_OPENGLORIENTATION
816                 out->m[0][0] = (cy) * scale;
817                 out->m[1][0] = (-sy) * scale;
818                 out->m[2][0] = 0;
819                 out->m[3][0] = x;
820                 out->m[0][1] = (sy) * scale;
821                 out->m[1][1] = (cy) * scale;
822                 out->m[2][1] = 0;
823                 out->m[3][1] = y;
824                 out->m[0][2] = 0;
825                 out->m[1][2] = 0;
826                 out->m[2][2] = scale;
827                 out->m[3][2] = z;
828                 out->m[0][3] = 0;
829                 out->m[1][3] = 0;
830                 out->m[2][3] = 0;
831                 out->m[3][3] = 1;
832 #else
833                 out->m[0][0] = (cy) * scale;
834                 out->m[0][1] = (-sy) * scale;
835                 out->m[0][2] = 0;
836                 out->m[0][3] = x;
837                 out->m[1][0] = (sy) * scale;
838                 out->m[1][1] = (cy) * scale;
839                 out->m[1][2] = 0;
840                 out->m[1][3] = y;
841                 out->m[2][0] = 0;
842                 out->m[2][1] = 0;
843                 out->m[2][2] = scale;
844                 out->m[2][3] = z;
845                 out->m[3][0] = 0;
846                 out->m[3][1] = 0;
847                 out->m[3][2] = 0;
848                 out->m[3][3] = 1;
849 #endif
850         }
851         else
852         {
853 #ifdef MATRIX4x4_OPENGLORIENTATION
854                 out->m[0][0] = scale;
855                 out->m[1][0] = 0;
856                 out->m[2][0] = 0;
857                 out->m[3][0] = x;
858                 out->m[0][1] = 0;
859                 out->m[1][1] = scale;
860                 out->m[2][1] = 0;
861                 out->m[3][1] = y;
862                 out->m[0][2] = 0;
863                 out->m[1][2] = 0;
864                 out->m[2][2] = scale;
865                 out->m[3][2] = z;
866                 out->m[0][3] = 0;
867                 out->m[1][3] = 0;
868                 out->m[2][3] = 0;
869                 out->m[3][3] = 1;
870 #else
871                 out->m[0][0] = scale;
872                 out->m[0][1] = 0;
873                 out->m[0][2] = 0;
874                 out->m[0][3] = x;
875                 out->m[1][0] = 0;
876                 out->m[1][1] = scale;
877                 out->m[1][2] = 0;
878                 out->m[1][3] = y;
879                 out->m[2][0] = 0;
880                 out->m[2][1] = 0;
881                 out->m[2][2] = scale;
882                 out->m[2][3] = z;
883                 out->m[3][0] = 0;
884                 out->m[3][1] = 0;
885                 out->m[3][2] = 0;
886                 out->m[3][3] = 1;
887 #endif
888         }
889 }
890
891 void Matrix4x4_QuakeToDuke3D(const matrix4x4_t *in, matrix4x4_t *out, double maxShearAngle)
892 {
893         // Sorry - this isn't direct at all. We can't just use an alternative to
894         // Matrix4x4_CreateFromQuakeEntity as in some cases the input for
895         // generating the view matrix is generated externally.
896         vec3_t forward, left, up, angles;
897         double scaleforward, scaleleft, scaleup;
898 #ifdef MATRIX4x4_OPENGLORIENTATION
899         VectorSet(forward, in->m[0][0], in->m[0][1], in->m[0][2]);
900         VectorSet(left, in->m[1][0], in->m[1][1], in->m[1][2]);
901         VectorSet(up, in->m[2][0], in->m[2][1], in->m[2][2]);
902 #else
903         VectorSet(forward, in->m[0][0], in->m[1][0], in->m[2][0]);
904         VectorSet(left, in->m[0][1], in->m[1][1], in->m[2][1]);
905         VectorSet(up, in->m[0][2], in->m[1][2], in->m[2][2]);
906 #endif
907         scaleforward = VectorNormalizeLength(forward);
908         scaleleft = VectorNormalizeLength(left);
909         scaleup = VectorNormalizeLength(up);
910         AnglesFromVectors(angles, forward, up, false);
911         AngleVectorsDuke3DFLU(angles, forward, left, up, maxShearAngle);
912         VectorScale(forward, scaleforward, forward);
913         VectorScale(left, scaleleft, left);
914         VectorScale(up, scaleup, up);
915         *out = *in;
916 #ifdef MATRIX4x4_OPENGLORIENTATION
917         out->m[0][0] = forward[0];
918         out->m[1][0] = left[0];
919         out->m[2][0] = up[0];
920         out->m[0][1] = forward[1];
921         out->m[1][1] = left[1];
922         out->m[2][1] = up[1];
923         out->m[0][2] = forward[2];
924         out->m[1][2] = left[2];
925         out->m[2][2] = up[2];
926 #else
927         out->m[0][0] = forward[0];
928         out->m[0][1] = left[0];
929         out->m[0][2] = up[0];
930         out->m[1][0] = forward[1];
931         out->m[1][1] = left[1];
932         out->m[1][2] = up[1];
933         out->m[2][0] = forward[2];
934         out->m[2][1] = left[2];
935         out->m[2][2] = up[2];
936 #endif
937 }
938
939 void Matrix4x4_ToVectors(const matrix4x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
940 {
941 #ifdef MATRIX4x4_OPENGLORIENTATION
942         vx[0] = in->m[0][0];
943         vx[1] = in->m[0][1];
944         vx[2] = in->m[0][2];
945         vy[0] = in->m[1][0];
946         vy[1] = in->m[1][1];
947         vy[2] = in->m[1][2];
948         vz[0] = in->m[2][0];
949         vz[1] = in->m[2][1];
950         vz[2] = in->m[2][2];
951         t [0] = in->m[3][0];
952         t [1] = in->m[3][1];
953         t [2] = in->m[3][2];
954 #else
955         vx[0] = in->m[0][0];
956         vx[1] = in->m[1][0];
957         vx[2] = in->m[2][0];
958         vy[0] = in->m[0][1];
959         vy[1] = in->m[1][1];
960         vy[2] = in->m[2][1];
961         vz[0] = in->m[0][2];
962         vz[1] = in->m[1][2];
963         vz[2] = in->m[2][2];
964         t [0] = in->m[0][3];
965         t [1] = in->m[1][3];
966         t [2] = in->m[2][3];
967 #endif
968 }
969
970 void Matrix4x4_FromVectors(matrix4x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
971 {
972 #ifdef MATRIX4x4_OPENGLORIENTATION
973         out->m[0][0] = vx[0];
974         out->m[1][0] = vy[0];
975         out->m[2][0] = vz[0];
976         out->m[3][0] = t[0];
977         out->m[0][1] = vx[1];
978         out->m[1][1] = vy[1];
979         out->m[2][1] = vz[1];
980         out->m[3][1] = t[1];
981         out->m[0][2] = vx[2];
982         out->m[1][2] = vy[2];
983         out->m[2][2] = vz[2];
984         out->m[3][2] = t[2];
985         out->m[0][3] = 0.0f;
986         out->m[1][3] = 0.0f;
987         out->m[2][3] = 0.0f;
988         out->m[3][3] = 1.0f;
989 #else
990         out->m[0][0] = vx[0];
991         out->m[0][1] = vy[0];
992         out->m[0][2] = vz[0];
993         out->m[0][3] = t[0];
994         out->m[1][0] = vx[1];
995         out->m[1][1] = vy[1];
996         out->m[1][2] = vz[1];
997         out->m[1][3] = t[1];
998         out->m[2][0] = vx[2];
999         out->m[2][1] = vy[2];
1000         out->m[2][2] = vz[2];
1001         out->m[2][3] = t[2];
1002         out->m[3][0] = 0.0f;
1003         out->m[3][1] = 0.0f;
1004         out->m[3][2] = 0.0f;
1005         out->m[3][3] = 1.0f;
1006 #endif
1007 }
1008
1009 void Matrix4x4_ToArrayDoubleGL(const matrix4x4_t *in, double out[16])
1010 {
1011 #ifdef MATRIX4x4_OPENGLORIENTATION
1012         out[ 0] = in->m[0][0];
1013         out[ 1] = in->m[0][1];
1014         out[ 2] = in->m[0][2];
1015         out[ 3] = in->m[0][3];
1016         out[ 4] = in->m[1][0];
1017         out[ 5] = in->m[1][1];
1018         out[ 6] = in->m[1][2];
1019         out[ 7] = in->m[1][3];
1020         out[ 8] = in->m[2][0];
1021         out[ 9] = in->m[2][1];
1022         out[10] = in->m[2][2];
1023         out[11] = in->m[2][3];
1024         out[12] = in->m[3][0];
1025         out[13] = in->m[3][1];
1026         out[14] = in->m[3][2];
1027         out[15] = in->m[3][3];
1028 #else
1029         out[ 0] = in->m[0][0];
1030         out[ 1] = in->m[1][0];
1031         out[ 2] = in->m[2][0];
1032         out[ 3] = in->m[3][0];
1033         out[ 4] = in->m[0][1];
1034         out[ 5] = in->m[1][1];
1035         out[ 6] = in->m[2][1];
1036         out[ 7] = in->m[3][1];
1037         out[ 8] = in->m[0][2];
1038         out[ 9] = in->m[1][2];
1039         out[10] = in->m[2][2];
1040         out[11] = in->m[3][2];
1041         out[12] = in->m[0][3];
1042         out[13] = in->m[1][3];
1043         out[14] = in->m[2][3];
1044         out[15] = in->m[3][3];
1045 #endif
1046 }
1047
1048 void Matrix4x4_FromArrayDoubleGL (matrix4x4_t *out, const double in[16])
1049 {
1050 #ifdef MATRIX4x4_OPENGLORIENTATION
1051         out->m[0][0] = in[0];
1052         out->m[0][1] = in[1];
1053         out->m[0][2] = in[2];
1054         out->m[0][3] = in[3];
1055         out->m[1][0] = in[4];
1056         out->m[1][1] = in[5];
1057         out->m[1][2] = in[6];
1058         out->m[1][3] = in[7];
1059         out->m[2][0] = in[8];
1060         out->m[2][1] = in[9];
1061         out->m[2][2] = in[10];
1062         out->m[2][3] = in[11];
1063         out->m[3][0] = in[12];
1064         out->m[3][1] = in[13];
1065         out->m[3][2] = in[14];
1066         out->m[3][3] = in[15];
1067 #else
1068         out->m[0][0] = in[0];
1069         out->m[1][0] = in[1];
1070         out->m[2][0] = in[2];
1071         out->m[3][0] = in[3];
1072         out->m[0][1] = in[4];
1073         out->m[1][1] = in[5];
1074         out->m[2][1] = in[6];
1075         out->m[3][1] = in[7];
1076         out->m[0][2] = in[8];
1077         out->m[1][2] = in[9];
1078         out->m[2][2] = in[10];
1079         out->m[3][2] = in[11];
1080         out->m[0][3] = in[12];
1081         out->m[1][3] = in[13];
1082         out->m[2][3] = in[14];
1083         out->m[3][3] = in[15];
1084 #endif
1085 }
1086
1087 void Matrix4x4_ToArrayDoubleD3D(const matrix4x4_t *in, double out[16])
1088 {
1089 #ifdef MATRIX4x4_OPENGLORIENTATION
1090         out[ 0] = in->m[0][0];
1091         out[ 1] = in->m[1][0];
1092         out[ 2] = in->m[2][0];
1093         out[ 3] = in->m[3][0];
1094         out[ 4] = in->m[0][1];
1095         out[ 5] = in->m[1][1];
1096         out[ 6] = in->m[2][1];
1097         out[ 7] = in->m[3][1];
1098         out[ 8] = in->m[0][2];
1099         out[ 9] = in->m[1][2];
1100         out[10] = in->m[2][2];
1101         out[11] = in->m[3][2];
1102         out[12] = in->m[0][3];
1103         out[13] = in->m[1][3];
1104         out[14] = in->m[2][3];
1105         out[15] = in->m[3][3];
1106 #else
1107         out[ 0] = in->m[0][0];
1108         out[ 1] = in->m[0][1];
1109         out[ 2] = in->m[0][2];
1110         out[ 3] = in->m[0][3];
1111         out[ 4] = in->m[1][0];
1112         out[ 5] = in->m[1][1];
1113         out[ 6] = in->m[1][2];
1114         out[ 7] = in->m[1][3];
1115         out[ 8] = in->m[2][0];
1116         out[ 9] = in->m[2][1];
1117         out[10] = in->m[2][2];
1118         out[11] = in->m[2][3];
1119         out[12] = in->m[3][0];
1120         out[13] = in->m[3][1];
1121         out[14] = in->m[3][2];
1122         out[15] = in->m[3][3];
1123 #endif
1124 }
1125
1126 void Matrix4x4_FromArrayDoubleD3D (matrix4x4_t *out, const double in[16])
1127 {
1128 #ifdef MATRIX4x4_OPENGLORIENTATION
1129         out->m[0][0] = in[0];
1130         out->m[1][0] = in[1];
1131         out->m[2][0] = in[2];
1132         out->m[3][0] = in[3];
1133         out->m[0][1] = in[4];
1134         out->m[1][1] = in[5];
1135         out->m[2][1] = in[6];
1136         out->m[3][1] = in[7];
1137         out->m[0][2] = in[8];
1138         out->m[1][2] = in[9];
1139         out->m[2][2] = in[10];
1140         out->m[3][2] = in[11];
1141         out->m[0][3] = in[12];
1142         out->m[1][3] = in[13];
1143         out->m[2][3] = in[14];
1144         out->m[3][3] = in[15];
1145 #else
1146         out->m[0][0] = in[0];
1147         out->m[0][1] = in[1];
1148         out->m[0][2] = in[2];
1149         out->m[0][3] = in[3];
1150         out->m[1][0] = in[4];
1151         out->m[1][1] = in[5];
1152         out->m[1][2] = in[6];
1153         out->m[1][3] = in[7];
1154         out->m[2][0] = in[8];
1155         out->m[2][1] = in[9];
1156         out->m[2][2] = in[10];
1157         out->m[2][3] = in[11];
1158         out->m[3][0] = in[12];
1159         out->m[3][1] = in[13];
1160         out->m[3][2] = in[14];
1161         out->m[3][3] = in[15];
1162 #endif
1163 }
1164
1165 void Matrix4x4_ToArrayFloatGL(const matrix4x4_t *in, float out[16])
1166 {
1167 #ifdef MATRIX4x4_OPENGLORIENTATION
1168         out[ 0] = in->m[0][0];
1169         out[ 1] = in->m[0][1];
1170         out[ 2] = in->m[0][2];
1171         out[ 3] = in->m[0][3];
1172         out[ 4] = in->m[1][0];
1173         out[ 5] = in->m[1][1];
1174         out[ 6] = in->m[1][2];
1175         out[ 7] = in->m[1][3];
1176         out[ 8] = in->m[2][0];
1177         out[ 9] = in->m[2][1];
1178         out[10] = in->m[2][2];
1179         out[11] = in->m[2][3];
1180         out[12] = in->m[3][0];
1181         out[13] = in->m[3][1];
1182         out[14] = in->m[3][2];
1183         out[15] = in->m[3][3];
1184 #else
1185         out[ 0] = in->m[0][0];
1186         out[ 1] = in->m[1][0];
1187         out[ 2] = in->m[2][0];
1188         out[ 3] = in->m[3][0];
1189         out[ 4] = in->m[0][1];
1190         out[ 5] = in->m[1][1];
1191         out[ 6] = in->m[2][1];
1192         out[ 7] = in->m[3][1];
1193         out[ 8] = in->m[0][2];
1194         out[ 9] = in->m[1][2];
1195         out[10] = in->m[2][2];
1196         out[11] = in->m[3][2];
1197         out[12] = in->m[0][3];
1198         out[13] = in->m[1][3];
1199         out[14] = in->m[2][3];
1200         out[15] = in->m[3][3];
1201 #endif
1202 }
1203
1204 void Matrix4x4_FromArrayFloatGL (matrix4x4_t *out, const float in[16])
1205 {
1206 #ifdef MATRIX4x4_OPENGLORIENTATION
1207         out->m[0][0] = in[0];
1208         out->m[0][1] = in[1];
1209         out->m[0][2] = in[2];
1210         out->m[0][3] = in[3];
1211         out->m[1][0] = in[4];
1212         out->m[1][1] = in[5];
1213         out->m[1][2] = in[6];
1214         out->m[1][3] = in[7];
1215         out->m[2][0] = in[8];
1216         out->m[2][1] = in[9];
1217         out->m[2][2] = in[10];
1218         out->m[2][3] = in[11];
1219         out->m[3][0] = in[12];
1220         out->m[3][1] = in[13];
1221         out->m[3][2] = in[14];
1222         out->m[3][3] = in[15];
1223 #else
1224         out->m[0][0] = in[0];
1225         out->m[1][0] = in[1];
1226         out->m[2][0] = in[2];
1227         out->m[3][0] = in[3];
1228         out->m[0][1] = in[4];
1229         out->m[1][1] = in[5];
1230         out->m[2][1] = in[6];
1231         out->m[3][1] = in[7];
1232         out->m[0][2] = in[8];
1233         out->m[1][2] = in[9];
1234         out->m[2][2] = in[10];
1235         out->m[3][2] = in[11];
1236         out->m[0][3] = in[12];
1237         out->m[1][3] = in[13];
1238         out->m[2][3] = in[14];
1239         out->m[3][3] = in[15];
1240 #endif
1241 }
1242
1243 void Matrix4x4_ToArrayFloatD3D(const matrix4x4_t *in, float out[16])
1244 {
1245 #ifdef MATRIX4x4_OPENGLORIENTATION
1246         out[ 0] = in->m[0][0];
1247         out[ 1] = in->m[1][0];
1248         out[ 2] = in->m[2][0];
1249         out[ 3] = in->m[3][0];
1250         out[ 4] = in->m[0][1];
1251         out[ 5] = in->m[1][1];
1252         out[ 6] = in->m[2][1];
1253         out[ 7] = in->m[3][1];
1254         out[ 8] = in->m[0][2];
1255         out[ 9] = in->m[1][2];
1256         out[10] = in->m[2][2];
1257         out[11] = in->m[3][2];
1258         out[12] = in->m[0][3];
1259         out[13] = in->m[1][3];
1260         out[14] = in->m[2][3];
1261         out[15] = in->m[3][3];
1262 #else
1263         out[ 0] = in->m[0][0];
1264         out[ 1] = in->m[0][1];
1265         out[ 2] = in->m[0][2];
1266         out[ 3] = in->m[0][3];
1267         out[ 4] = in->m[1][0];
1268         out[ 5] = in->m[1][1];
1269         out[ 6] = in->m[1][2];
1270         out[ 7] = in->m[1][3];
1271         out[ 8] = in->m[2][0];
1272         out[ 9] = in->m[2][1];
1273         out[10] = in->m[2][2];
1274         out[11] = in->m[2][3];
1275         out[12] = in->m[3][0];
1276         out[13] = in->m[3][1];
1277         out[14] = in->m[3][2];
1278         out[15] = in->m[3][3];
1279 #endif
1280 }
1281
1282 void Matrix4x4_FromArrayFloatD3D (matrix4x4_t *out, const float in[16])
1283 {
1284 #ifdef MATRIX4x4_OPENGLORIENTATION
1285         out->m[0][0] = in[0];
1286         out->m[1][0] = in[1];
1287         out->m[2][0] = in[2];
1288         out->m[3][0] = in[3];
1289         out->m[0][1] = in[4];
1290         out->m[1][1] = in[5];
1291         out->m[2][1] = in[6];
1292         out->m[3][1] = in[7];
1293         out->m[0][2] = in[8];
1294         out->m[1][2] = in[9];
1295         out->m[2][2] = in[10];
1296         out->m[3][2] = in[11];
1297         out->m[0][3] = in[12];
1298         out->m[1][3] = in[13];
1299         out->m[2][3] = in[14];
1300         out->m[3][3] = in[15];
1301 #else
1302         out->m[0][0] = in[0];
1303         out->m[0][1] = in[1];
1304         out->m[0][2] = in[2];
1305         out->m[0][3] = in[3];
1306         out->m[1][0] = in[4];
1307         out->m[1][1] = in[5];
1308         out->m[1][2] = in[6];
1309         out->m[1][3] = in[7];
1310         out->m[2][0] = in[8];
1311         out->m[2][1] = in[9];
1312         out->m[2][2] = in[10];
1313         out->m[2][3] = in[11];
1314         out->m[3][0] = in[12];
1315         out->m[3][1] = in[13];
1316         out->m[3][2] = in[14];
1317         out->m[3][3] = in[15];
1318 #endif
1319 }
1320
1321 void Matrix4x4_ToArray12FloatGL(const matrix4x4_t *in, float out[12])
1322 {
1323 #ifdef MATRIX4x4_OPENGLORIENTATION
1324         out[ 0] = in->m[0][0];
1325         out[ 1] = in->m[0][1];
1326         out[ 2] = in->m[0][2];
1327         out[ 3] = in->m[1][0];
1328         out[ 4] = in->m[1][1];
1329         out[ 5] = in->m[1][2];
1330         out[ 6] = in->m[2][0];
1331         out[ 7] = in->m[2][1];
1332         out[ 8] = in->m[2][2];
1333         out[ 9] = in->m[3][0];
1334         out[10] = in->m[3][1];
1335         out[11] = in->m[3][2];
1336 #else
1337         out[ 0] = in->m[0][0];
1338         out[ 1] = in->m[1][0];
1339         out[ 2] = in->m[2][0];
1340         out[ 3] = in->m[0][1];
1341         out[ 4] = in->m[1][1];
1342         out[ 5] = in->m[2][1];
1343         out[ 6] = in->m[0][2];
1344         out[ 7] = in->m[1][2];
1345         out[ 8] = in->m[2][2];
1346         out[ 9] = in->m[0][3];
1347         out[10] = in->m[1][3];
1348         out[11] = in->m[2][3];
1349 #endif
1350 }
1351
1352 void Matrix4x4_FromArray12FloatGL(matrix4x4_t *out, const float in[12])
1353 {
1354 #ifdef MATRIX4x4_OPENGLORIENTATION
1355         out->m[0][0] = in[0];
1356         out->m[0][1] = in[1];
1357         out->m[0][2] = in[2];
1358         out->m[0][3] = 0;
1359         out->m[1][0] = in[3];
1360         out->m[1][1] = in[4];
1361         out->m[1][2] = in[5];
1362         out->m[1][3] = 0;
1363         out->m[2][0] = in[6];
1364         out->m[2][1] = in[7];
1365         out->m[2][2] = in[8];
1366         out->m[2][3] = 0;
1367         out->m[3][0] = in[9];
1368         out->m[3][1] = in[10];
1369         out->m[3][2] = in[11];
1370         out->m[3][3] = 1;
1371 #else
1372         out->m[0][0] = in[0];
1373         out->m[1][0] = in[1];
1374         out->m[2][0] = in[2];
1375         out->m[3][0] = 0;
1376         out->m[0][1] = in[3];
1377         out->m[1][1] = in[4];
1378         out->m[2][1] = in[5];
1379         out->m[3][1] = 0;
1380         out->m[0][2] = in[6];
1381         out->m[1][2] = in[7];
1382         out->m[2][2] = in[8];
1383         out->m[3][2] = 0;
1384         out->m[0][3] = in[9];
1385         out->m[1][3] = in[10];
1386         out->m[2][3] = in[11];
1387         out->m[3][3] = 1;
1388 #endif
1389 }
1390
1391 void Matrix4x4_ToArray12FloatD3D(const matrix4x4_t *in, float out[12])
1392 {
1393 #ifdef MATRIX4x4_OPENGLORIENTATION
1394         out[ 0] = in->m[0][0];
1395         out[ 1] = in->m[1][0];
1396         out[ 2] = in->m[2][0];
1397         out[ 3] = in->m[3][0];
1398         out[ 4] = in->m[0][1];
1399         out[ 5] = in->m[1][1];
1400         out[ 6] = in->m[2][1];
1401         out[ 7] = in->m[3][1];
1402         out[ 8] = in->m[0][2];
1403         out[ 9] = in->m[1][2];
1404         out[10] = in->m[2][2];
1405         out[11] = in->m[3][2];
1406 #else
1407         out[ 0] = in->m[0][0];
1408         out[ 1] = in->m[0][1];
1409         out[ 2] = in->m[0][2];
1410         out[ 3] = in->m[0][3];
1411         out[ 4] = in->m[1][0];
1412         out[ 5] = in->m[1][1];
1413         out[ 6] = in->m[1][2];
1414         out[ 7] = in->m[1][3];
1415         out[ 8] = in->m[2][0];
1416         out[ 9] = in->m[2][1];
1417         out[10] = in->m[2][2];
1418         out[11] = in->m[2][3];
1419 #endif
1420 }
1421
1422 void Matrix4x4_FromArray12FloatD3D(matrix4x4_t *out, const float in[12])
1423 {
1424 #ifdef MATRIX4x4_OPENGLORIENTATION
1425         out->m[0][0] = in[0];
1426         out->m[1][0] = in[1];
1427         out->m[2][0] = in[2];
1428         out->m[3][0] = in[3];
1429         out->m[0][1] = in[4];
1430         out->m[1][1] = in[5];
1431         out->m[2][1] = in[6];
1432         out->m[3][1] = in[7];
1433         out->m[0][2] = in[8];
1434         out->m[1][2] = in[9];
1435         out->m[2][2] = in[10];
1436         out->m[3][2] = in[11];
1437         out->m[0][3] = 0;
1438         out->m[1][3] = 0;
1439         out->m[2][3] = 0;
1440         out->m[3][3] = 1;
1441 #else
1442         out->m[0][0] = in[0];
1443         out->m[0][1] = in[1];
1444         out->m[0][2] = in[2];
1445         out->m[0][3] = in[3];
1446         out->m[1][0] = in[4];
1447         out->m[1][1] = in[5];
1448         out->m[1][2] = in[6];
1449         out->m[1][3] = in[7];
1450         out->m[2][0] = in[8];
1451         out->m[2][1] = in[9];
1452         out->m[2][2] = in[10];
1453         out->m[2][3] = in[11];
1454         out->m[3][0] = 0;
1455         out->m[3][1] = 0;
1456         out->m[3][2] = 0;
1457         out->m[3][3] = 1;
1458 #endif
1459 }
1460
1461 void Matrix4x4_FromOriginQuat(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z, double w)
1462 {
1463 #ifdef MATRIX4x4_OPENGLORIENTATION
1464         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;
1465         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;
1466         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;
1467         m->m[0][3]=  0          ;m->m[1][3]=  0          ;m->m[2][3]=  0          ;m->m[3][3]=1;
1468 #else
1469         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;
1470         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;
1471         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;
1472         m->m[3][0]=  0          ;m->m[3][1]=  0          ;m->m[3][2]=  0          ;m->m[3][3]=1;
1473 #endif
1474 }
1475
1476 // see http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
1477 void Matrix4x4_ToOrigin3Quat4Float(const matrix4x4_t *m, float *origin, float *quat)
1478 {
1479 #if 0
1480         float s;
1481         quat[3] = sqrt(1.0f + m->m[0][0] + m->m[1][1] + m->m[2][2]) * 0.5f;
1482         s = 0.25f / quat[3];
1483 #ifdef MATRIX4x4_OPENGLORIENTATION
1484         origin[0] = m->m[3][0];
1485         origin[1] = m->m[3][1];
1486         origin[2] = m->m[3][2];
1487         quat[0] = (m->m[1][2] - m->m[2][1]) * s;
1488         quat[1] = (m->m[2][0] - m->m[0][2]) * s;
1489         quat[2] = (m->m[0][1] - m->m[1][0]) * s;
1490 #else
1491         origin[0] = m->m[0][3];
1492         origin[1] = m->m[1][3];
1493         origin[2] = m->m[2][3];
1494         quat[0] = (m->m[2][1] - m->m[1][2]) * s;
1495         quat[1] = (m->m[0][2] - m->m[2][0]) * s;
1496         quat[2] = (m->m[1][0] - m->m[0][1]) * s;
1497 #endif
1498
1499 #else
1500
1501 #ifdef MATRIX4x4_OPENGLORIENTATION
1502         float trace = m->m[0][0] + m->m[1][1] + m->m[2][2];
1503         origin[0] = m->m[3][0];
1504         origin[1] = m->m[3][1];
1505         origin[2] = m->m[3][2];
1506         if(trace > 0)
1507         {
1508                 float r = sqrt(1.0f + trace), inv = 0.5f / r;
1509                 quat[0] = (m->m[1][2] - m->m[2][1]) * inv;
1510                 quat[1] = (m->m[2][0] - m->m[0][2]) * inv;
1511                 quat[2] = (m->m[0][1] - m->m[1][0]) * inv;
1512                 quat[3] = 0.5f * r;
1513         }
1514         else if(m->m[0][0] > m->m[1][1] && m->m[0][0] > m->m[2][2])
1515         {
1516                 float r = sqrt(1.0f + m->m[0][0] - m->m[1][1] - m->m[2][2]), inv = 0.5f / r;
1517                 quat[0] = 0.5f * r;
1518                 quat[1] = (m->m[0][1] + m->m[1][0]) * inv;
1519                 quat[2] = (m->m[2][0] + m->m[0][2]) * inv;
1520                 quat[3] = (m->m[1][2] - m->m[2][1]) * inv;
1521         }
1522         else if(m->m[1][1] > m->m[2][2])
1523         {
1524                 float r = sqrt(1.0f + m->m[1][1] - m->m[0][0] - m->m[2][2]), inv = 0.5f / r;
1525                 quat[0] = (m->m[0][1] + m->m[1][0]) * inv;
1526                 quat[1] = 0.5f * r;
1527                 quat[2] = (m->m[1][2] + m->m[2][1]) * inv;
1528                 quat[3] = (m->m[2][0] - m->m[0][2]) * inv;
1529         }
1530         else
1531         {
1532                 float r = sqrt(1.0f + m->m[2][2] - m->m[0][0] - m->m[1][1]), inv = 0.5f / r;
1533                 quat[0] = (m->m[2][0] + m->m[0][2]) * inv;
1534                 quat[1] = (m->m[1][2] + m->m[2][1]) * inv;
1535                 quat[2] = 0.5f * r;
1536                 quat[3] = (m->m[0][1] - m->m[1][0]) * inv;
1537         }
1538 #else
1539         float trace = m->m[0][0] + m->m[1][1] + m->m[2][2];
1540         origin[0] = m->m[0][3];
1541         origin[1] = m->m[1][3];
1542         origin[2] = m->m[2][3];
1543         if(trace > 0)
1544         {
1545                 float r = sqrt(1.0f + trace), inv = 0.5f / r;
1546                 quat[0] = (m->m[2][1] - m->m[1][2]) * inv;
1547                 quat[1] = (m->m[0][2] - m->m[2][0]) * inv;
1548                 quat[2] = (m->m[1][0] - m->m[0][1]) * inv;
1549                 quat[3] = 0.5f * r;
1550         }
1551         else if(m->m[0][0] > m->m[1][1] && m->m[0][0] > m->m[2][2])
1552         {
1553                 float r = sqrt(1.0f + m->m[0][0] - m->m[1][1] - m->m[2][2]), inv = 0.5f / r;
1554                 quat[0] = 0.5f * r;
1555                 quat[1] = (m->m[1][0] + m->m[0][1]) * inv;
1556                 quat[2] = (m->m[0][2] + m->m[2][0]) * inv;
1557                 quat[3] = (m->m[2][1] - m->m[1][2]) * inv;
1558         }
1559         else if(m->m[1][1] > m->m[2][2])
1560         {
1561                 float r = sqrt(1.0f + m->m[1][1] - m->m[0][0] - m->m[2][2]), inv = 0.5f / r;
1562                 quat[0] = (m->m[1][0] + m->m[0][1]) * inv;
1563                 quat[1] = 0.5f * r;
1564                 quat[2] = (m->m[2][1] + m->m[1][2]) * inv;
1565                 quat[3] = (m->m[0][2] - m->m[2][0]) * inv;
1566         }
1567         else
1568         {
1569                 float r = sqrt(1.0f + m->m[2][2] - m->m[0][0] - m->m[1][1]), inv = 0.5f / r;
1570                 quat[0] = (m->m[0][2] + m->m[2][0]) * inv;
1571                 quat[1] = (m->m[2][1] + m->m[1][2]) * inv;
1572                 quat[2] = 0.5f * r;
1573                 quat[3] = (m->m[1][0] - m->m[0][1]) * inv;
1574         }
1575 #endif
1576
1577 #endif
1578 }
1579
1580 // LadyHavoc: I got this code from:
1581 //http://www.doom3world.org/phpbb2/viewtopic.php?t=2884
1582 void Matrix4x4_FromDoom3Joint(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z)
1583 {
1584         double w = 1.0f - (x*x+y*y+z*z);
1585         w = w > 0.0f ? -sqrt(w) : 0.0f;
1586 #ifdef MATRIX4x4_OPENGLORIENTATION
1587         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;
1588         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;
1589         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;
1590         m->m[0][3]=  0          ;m->m[1][3]=  0          ;m->m[2][3]=  0          ;m->m[3][3]=1;
1591 #else
1592         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;
1593         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;
1594         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;
1595         m->m[3][0]=  0          ;m->m[3][1]=  0          ;m->m[3][2]=  0          ;m->m[3][3]=1;
1596 #endif
1597 }
1598
1599 void Matrix4x4_FromBonePose7s(matrix4x4_t *m, float originscale, const short *pose7s)
1600 {
1601         float origin[3];
1602         float quat[4];
1603         float quatscale = pose7s[6] > 0 ? -1.0f / 32767.0f : 1.0f / 32767.0f;
1604         origin[0] = pose7s[0] * originscale;
1605         origin[1] = pose7s[1] * originscale;
1606         origin[2] = pose7s[2] * originscale;
1607         quat[0] = pose7s[3] * quatscale;
1608         quat[1] = pose7s[4] * quatscale;
1609         quat[2] = pose7s[5] * quatscale;
1610         quat[3] = pose7s[6] * quatscale;
1611         Matrix4x4_FromOriginQuat(m, origin[0], origin[1], origin[2], quat[0], quat[1], quat[2], quat[3]);
1612 }
1613
1614 void Matrix4x4_ToBonePose7s(const matrix4x4_t *m, float origininvscale, short *pose7s)
1615 {
1616         float origin[3];
1617         float quat[4];
1618         float quatscale;
1619         Matrix4x4_ToOrigin3Quat4Float(m, origin, quat);
1620         // normalize quaternion so that it is unit length
1621         quatscale = quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3];
1622         if (quatscale)
1623                 quatscale = (quat[3] >= 0 ? -32767.0f : 32767.0f) / sqrt(quatscale);
1624         // use a negative scale on the quat because the above function produces a
1625         // positive quat[3] and canonical quaternions have negative quat[3]
1626         pose7s[0] = origin[0] * origininvscale;
1627         pose7s[1] = origin[1] * origininvscale;
1628         pose7s[2] = origin[2] * origininvscale;
1629         pose7s[3] = quat[0] * quatscale;
1630         pose7s[4] = quat[1] * quatscale;
1631         pose7s[5] = quat[2] * quatscale;
1632         pose7s[6] = quat[3] * quatscale;
1633 }
1634
1635 void Matrix4x4_Blend (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2, double blend)
1636 {
1637         double iblend = 1 - blend;
1638         out->m[0][0] = in1->m[0][0] * iblend + in2->m[0][0] * blend;
1639         out->m[0][1] = in1->m[0][1] * iblend + in2->m[0][1] * blend;
1640         out->m[0][2] = in1->m[0][2] * iblend + in2->m[0][2] * blend;
1641         out->m[0][3] = in1->m[0][3] * iblend + in2->m[0][3] * blend;
1642         out->m[1][0] = in1->m[1][0] * iblend + in2->m[1][0] * blend;
1643         out->m[1][1] = in1->m[1][1] * iblend + in2->m[1][1] * blend;
1644         out->m[1][2] = in1->m[1][2] * iblend + in2->m[1][2] * blend;
1645         out->m[1][3] = in1->m[1][3] * iblend + in2->m[1][3] * blend;
1646         out->m[2][0] = in1->m[2][0] * iblend + in2->m[2][0] * blend;
1647         out->m[2][1] = in1->m[2][1] * iblend + in2->m[2][1] * blend;
1648         out->m[2][2] = in1->m[2][2] * iblend + in2->m[2][2] * blend;
1649         out->m[2][3] = in1->m[2][3] * iblend + in2->m[2][3] * blend;
1650         out->m[3][0] = in1->m[3][0] * iblend + in2->m[3][0] * blend;
1651         out->m[3][1] = in1->m[3][1] * iblend + in2->m[3][1] * blend;
1652         out->m[3][2] = in1->m[3][2] * iblend + in2->m[3][2] * blend;
1653         out->m[3][3] = in1->m[3][3] * iblend + in2->m[3][3] * blend;
1654 }
1655
1656
1657 void Matrix4x4_Transform (const matrix4x4_t *in, const float v[3], float out[3])
1658 {
1659 #ifdef MATRIX4x4_OPENGLORIENTATION
1660         out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0] + in->m[3][0];
1661         out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1] + in->m[3][1];
1662         out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2] + in->m[3][2];
1663 #else
1664         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
1665         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
1666         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
1667 #endif
1668 }
1669
1670 void Matrix4x4_Transform4 (const matrix4x4_t *in, const float v[4], float out[4])
1671 {
1672 #ifdef MATRIX4x4_OPENGLORIENTATION
1673         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];
1674         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];
1675         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];
1676         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];
1677 #else
1678         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];
1679         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];
1680         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];
1681         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];
1682 #endif
1683 }
1684
1685 void Matrix4x4_Transform3x3 (const matrix4x4_t *in, const float v[3], float out[3])
1686 {
1687 #ifdef MATRIX4x4_OPENGLORIENTATION
1688         out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0];
1689         out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1];
1690         out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2];
1691 #else
1692         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2];
1693         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2];
1694         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2];
1695 #endif
1696 }
1697
1698 // transforms a positive distance plane (A*x+B*y+C*z-D=0) through a rotation or translation matrix
1699 void Matrix4x4_TransformPositivePlane(const matrix4x4_t *in, float x, float y, float z, float d, float *o)
1700 {
1701         float scale = 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]);
1702         float iscale = 1.0f / scale;
1703 #ifdef MATRIX4x4_OPENGLORIENTATION
1704         o[0] = (x * in->m[0][0] + y * in->m[1][0] + z * in->m[2][0]) * iscale;
1705         o[1] = (x * in->m[0][1] + y * in->m[1][1] + z * in->m[2][1]) * iscale;
1706         o[2] = (x * in->m[0][2] + y * in->m[1][2] + z * in->m[2][2]) * iscale;
1707         o[3] = d * scale + (o[0] * in->m[3][0] + o[1] * in->m[3][1] + o[2] * in->m[3][2]);
1708 #else
1709         o[0] = (x * in->m[0][0] + y * in->m[0][1] + z * in->m[0][2]) * iscale;
1710         o[1] = (x * in->m[1][0] + y * in->m[1][1] + z * in->m[1][2]) * iscale;
1711         o[2] = (x * in->m[2][0] + y * in->m[2][1] + z * in->m[2][2]) * iscale;
1712         o[3] = d * scale + (o[0] * in->m[0][3] + o[1] * in->m[1][3] + o[2] * in->m[2][3]);
1713 #endif
1714 }
1715
1716 // transforms a standard plane (A*x+B*y+C*z+D=0) through a rotation or translation matrix
1717 void Matrix4x4_TransformStandardPlane(const matrix4x4_t *in, float x, float y, float z, float d, float *o)
1718 {
1719         float scale = 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]);
1720         float iscale = 1.0f / scale;
1721 #ifdef MATRIX4x4_OPENGLORIENTATION
1722         o[0] = (x * in->m[0][0] + y * in->m[1][0] + z * in->m[2][0]) * iscale;
1723         o[1] = (x * in->m[0][1] + y * in->m[1][1] + z * in->m[2][1]) * iscale;
1724         o[2] = (x * in->m[0][2] + y * in->m[1][2] + z * in->m[2][2]) * iscale;
1725         o[3] = d * scale - (o[0] * in->m[3][0] + o[1] * in->m[3][1] + o[2] * in->m[3][2]);
1726 #else
1727         o[0] = (x * in->m[0][0] + y * in->m[0][1] + z * in->m[0][2]) * iscale;
1728         o[1] = (x * in->m[1][0] + y * in->m[1][1] + z * in->m[1][2]) * iscale;
1729         o[2] = (x * in->m[2][0] + y * in->m[2][1] + z * in->m[2][2]) * iscale;
1730         o[3] = d * scale - (o[0] * in->m[0][3] + o[1] * in->m[1][3] + o[2] * in->m[2][3]);
1731 #endif
1732 }
1733
1734 /*
1735 void Matrix4x4_SimpleUntransform (const matrix4x4_t *in, const float v[3], float out[3])
1736 {
1737         double t[3];
1738 #ifdef MATRIX4x4_OPENGLORIENTATION
1739         t[0] = v[0] - in->m[3][0];
1740         t[1] = v[1] - in->m[3][1];
1741         t[2] = v[2] - in->m[3][2];
1742         out[0] = t[0] * in->m[0][0] + t[1] * in->m[0][1] + t[2] * in->m[0][2];
1743         out[1] = t[0] * in->m[1][0] + t[1] * in->m[1][1] + t[2] * in->m[1][2];
1744         out[2] = t[0] * in->m[2][0] + t[1] * in->m[2][1] + t[2] * in->m[2][2];
1745 #else
1746         t[0] = v[0] - in->m[0][3];
1747         t[1] = v[1] - in->m[1][3];
1748         t[2] = v[2] - in->m[2][3];
1749         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
1750         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
1751         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
1752 #endif
1753 }
1754 */
1755
1756 // FIXME: optimize
1757 void Matrix4x4_ConcatTranslate (matrix4x4_t *out, double x, double y, double z)
1758 {
1759         matrix4x4_t base, temp;
1760         base = *out;
1761         Matrix4x4_CreateTranslate(&temp, x, y, z);
1762         Matrix4x4_Concat(out, &base, &temp);
1763 }
1764
1765 // FIXME: optimize
1766 void Matrix4x4_ConcatRotate (matrix4x4_t *out, double angle, double x, double y, double z)
1767 {
1768         matrix4x4_t base, temp;
1769         base = *out;
1770         Matrix4x4_CreateRotate(&temp, angle, x, y, z);
1771         Matrix4x4_Concat(out, &base, &temp);
1772 }
1773
1774 // FIXME: optimize
1775 void Matrix4x4_ConcatScale (matrix4x4_t *out, double x)
1776 {
1777         matrix4x4_t base, temp;
1778         base = *out;
1779         Matrix4x4_CreateScale(&temp, x);
1780         Matrix4x4_Concat(out, &base, &temp);
1781 }
1782
1783 // FIXME: optimize
1784 void Matrix4x4_ConcatScale3 (matrix4x4_t *out, double x, double y, double z)
1785 {
1786         matrix4x4_t base, temp;
1787         base = *out;
1788         Matrix4x4_CreateScale3(&temp, x, y, z);
1789         Matrix4x4_Concat(out, &base, &temp);
1790 }
1791
1792 void Matrix4x4_OriginFromMatrix (const matrix4x4_t *in, float *out)
1793 {
1794 #ifdef MATRIX4x4_OPENGLORIENTATION
1795         out[0] = in->m[3][0];
1796         out[1] = in->m[3][1];
1797         out[2] = in->m[3][2];
1798 #else
1799         out[0] = in->m[0][3];
1800         out[1] = in->m[1][3];
1801         out[2] = in->m[2][3];
1802 #endif
1803 }
1804
1805 double Matrix4x4_ScaleFromMatrix (const matrix4x4_t *in)
1806 {
1807         // we only support uniform scaling, so assume the first row is enough
1808         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]);
1809 }
1810
1811 void Matrix4x4_SetOrigin (matrix4x4_t *out, double x, double y, double z)
1812 {
1813 #ifdef MATRIX4x4_OPENGLORIENTATION
1814         out->m[3][0] = x;
1815         out->m[3][1] = y;
1816         out->m[3][2] = z;
1817 #else
1818         out->m[0][3] = x;
1819         out->m[1][3] = y;
1820         out->m[2][3] = z;
1821 #endif
1822 }
1823
1824 void Matrix4x4_AdjustOrigin (matrix4x4_t *out, double x, double y, double z)
1825 {
1826 #ifdef MATRIX4x4_OPENGLORIENTATION
1827         out->m[3][0] += x;
1828         out->m[3][1] += y;
1829         out->m[3][2] += z;
1830 #else
1831         out->m[0][3] += x;
1832         out->m[1][3] += y;
1833         out->m[2][3] += z;
1834 #endif
1835 }
1836
1837 void Matrix4x4_Scale (matrix4x4_t *out, double rotatescale, double originscale)
1838 {
1839         out->m[0][0] *= rotatescale;
1840         out->m[0][1] *= rotatescale;
1841         out->m[0][2] *= rotatescale;
1842         out->m[1][0] *= rotatescale;
1843         out->m[1][1] *= rotatescale;
1844         out->m[1][2] *= rotatescale;
1845         out->m[2][0] *= rotatescale;
1846         out->m[2][1] *= rotatescale;
1847         out->m[2][2] *= rotatescale;
1848 #ifdef MATRIX4x4_OPENGLORIENTATION
1849         out->m[3][0] *= originscale;
1850         out->m[3][1] *= originscale;
1851         out->m[3][2] *= originscale;
1852 #else
1853         out->m[0][3] *= originscale;
1854         out->m[1][3] *= originscale;
1855         out->m[2][3] *= originscale;
1856 #endif
1857 }
1858
1859 void Matrix4x4_Abs (matrix4x4_t *out)
1860 {
1861         out->m[0][0] = fabs(out->m[0][0]);
1862         out->m[0][1] = fabs(out->m[0][1]);
1863         out->m[0][2] = fabs(out->m[0][2]);
1864         out->m[1][0] = fabs(out->m[1][0]);
1865         out->m[1][1] = fabs(out->m[1][1]);
1866         out->m[1][2] = fabs(out->m[1][2]);
1867         out->m[2][0] = fabs(out->m[2][0]);
1868         out->m[2][1] = fabs(out->m[2][1]);
1869         out->m[2][2] = fabs(out->m[2][2]);
1870 }
1871