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