DarkPlaces
Game engine based on the Quake 1 engine by id Software, developed by LadyHavoc
 
matrixlib.c
Go to the documentation of this file.
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
9const 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
19void Matrix4x4_Copy (matrix4x4_t *out, const matrix4x4_t *in)
20{
21 *out = *in;
22}
23
24void 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
44void 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
83void 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
122void 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
145int 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)
198int 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
253int 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
422void 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
475void 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
483void 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
491void 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
499void 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
508void 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
535void 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
564void 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
584void 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
623void 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
675void 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
695void 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
715void 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
891void 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
939void 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
970void 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
1009void 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
1048void 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
1087void 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
1126void Matrix4x4_FromArrayDoubleD3D (matrix4x4_t *out, const double in[4][4])
1127{
1128#ifdef MATRIX4x4_OPENGLORIENTATION
1129 out->m[0][0] = in[0][0];
1130 out->m[1][0] = in[0][1];
1131 out->m[2][0] = in[0][2];
1132 out->m[3][0] = in[0][3];
1133 out->m[0][1] = in[1][0];
1134 out->m[1][1] = in[1][1];
1135 out->m[2][1] = in[1][2];
1136 out->m[3][1] = in[1][3];
1137 out->m[0][2] = in[2][0];
1138 out->m[1][2] = in[2][1];
1139 out->m[2][2] = in[2][2];
1140 out->m[3][2] = in[2][3];
1141 out->m[0][3] = in[3][0];
1142 out->m[1][3] = in[3][1];
1143 out->m[2][3] = in[3][2];
1144 out->m[3][3] = in[3][3];
1145#else
1146 out->m[0][0] = in[0][0];
1147 out->m[0][1] = in[0][1];
1148 out->m[0][2] = in[0][2];
1149 out->m[0][3] = in[0][3];
1150 out->m[1][0] = in[1][0];
1151 out->m[1][1] = in[1][1];
1152 out->m[1][2] = in[1][2];
1153 out->m[1][3] = in[1][3];
1154 out->m[2][0] = in[2][0];
1155 out->m[2][1] = in[2][1];
1156 out->m[2][2] = in[2][2];
1157 out->m[2][3] = in[2][3];
1158 out->m[3][0] = in[3][0];
1159 out->m[3][1] = in[3][1];
1160 out->m[3][2] = in[3][2];
1161 out->m[3][3] = in[3][3];
1162#endif
1163}
1164
1165void 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
1204void 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
1243void 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
1282void 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
1321void Matrix4x4_ToArray12FloatGL(const matrix4x4_t *in, float out[4][3])
1322{
1323#ifdef MATRIX4x4_OPENGLORIENTATION
1324 out[0][0] = in->m[0][0];
1325 out[0][1] = in->m[0][1];
1326 out[0][2] = in->m[0][2];
1327 out[1][0] = in->m[1][0];
1328 out[1][1] = in->m[1][1];
1329 out[1][2] = in->m[1][2];
1330 out[2][0] = in->m[2][0];
1331 out[2][1] = in->m[2][1];
1332 out[2][2] = in->m[2][2];
1333 out[3][0] = in->m[3][0];
1334 out[3][1] = in->m[3][1];
1335 out[3][2] = in->m[3][2];
1336#else
1337 out[0][0] = in->m[0][0];
1338 out[0][1] = in->m[1][0];
1339 out[0][2] = in->m[2][0];
1340 out[1][0] = in->m[0][1];
1341 out[1][1] = in->m[1][1];
1342 out[1][2] = in->m[2][1];
1343 out[2][0] = in->m[0][2];
1344 out[2][1] = in->m[1][2];
1345 out[2][2] = in->m[2][2];
1346 out[3][0] = in->m[0][3];
1347 out[3][1] = in->m[1][3];
1348 out[3][2] = in->m[2][3];
1349#endif
1350}
1351
1352void 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
1391void 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
1422void 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
1461void 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
1477void 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
1582void 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
1599void 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
1614void 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
1635void 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
1657void 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
1670void 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
1685void 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
1699void 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
1717void 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/*
1735void 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
1757void 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
1766void 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
1775void 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
1784void 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
1792void 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
1805double 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
1811void 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
1824void 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
1837void 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
1859void 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
angles
vector angles
Definition csprogsdefs.qc:108
origin
vector origin
Definition csprogsdefs.qc:105
scale
float scale
Definition dpextensions.qc:282
z
GLubyte GLubyte GLubyte z
Definition glquake.h:782
w
GLubyte GLubyte GLubyte GLubyte w
Definition glquake.h:782
v
const GLdouble * v
Definition glquake.h:762
y
GLint GLenum GLint GLint y
Definition glquake.h:651
x
GLint GLenum GLint x
Definition glquake.h:651
AngleVectorsDuke3DFLU
void AngleVectorsDuke3DFLU(const vec3_t angles, vec3_t forward, vec3_t left, vec3_t up, double maxShearAngle)
divVerent: improper matrix version of AngleVectors
Definition mathlib.c:552
AnglesFromVectors
void AnglesFromVectors(vec3_t angles, const vec3_t forward, const vec3_t up, qbool flippitch)
LadyHavoc: calculates pitch/yaw/roll angles from forward and up vectors.
Definition mathlib.c:650
VectorNormalizeLength
float VectorNormalizeLength(vec3_t v)
returns vector length
Definition mathlib.c:763
VectorSet
#define VectorSet(vec, x, y, z)
Definition mathlib.h:96
VectorScale
#define VectorScale(in, scale, out)
Definition mathlib.h:111
M_PI
#define M_PI
Definition mathlib.h:28
Matrix4x4_AdjustOrigin
void Matrix4x4_AdjustOrigin(matrix4x4_t *out, double x, double y, double z)
Definition matrixlib.c:1824
Matrix4x4_Concat
void Matrix4x4_Concat(matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2)
Definition matrixlib.c:83
Matrix4x4_CreateIdentity
void Matrix4x4_CreateIdentity(matrix4x4_t *out)
Definition matrixlib.c:564
Matrix4x4_FromBonePose7s
void Matrix4x4_FromBonePose7s(matrix4x4_t *m, float originscale, const short *pose7s)
Definition matrixlib.c:1599
Matrix4x4_Clear
void Matrix4x4_Clear(matrix4x4_t *out)
Definition matrixlib.c:483
Matrix4x4_TransformStandardPlane
void Matrix4x4_TransformStandardPlane(const matrix4x4_t *in, float x, float y, float z, float d, float *o)
Definition matrixlib.c:1717
Matrix4x4_TransformPositivePlane
void Matrix4x4_TransformPositivePlane(const matrix4x4_t *in, float x, float y, float z, float d, float *o)
Definition matrixlib.c:1699
Matrix4x4_FromArray12FloatGL
void Matrix4x4_FromArray12FloatGL(matrix4x4_t *out, const float in[12])
Definition matrixlib.c:1352
Matrix4x4_Transform
void Matrix4x4_Transform(const matrix4x4_t *in, const float v[3], float out[3])
Definition matrixlib.c:1657
Matrix4x4_QuakeToDuke3D
void Matrix4x4_QuakeToDuke3D(const matrix4x4_t *in, matrix4x4_t *out, double maxShearAngle)
Definition matrixlib.c:891
Matrix4x4_Transform4
void Matrix4x4_Transform4(const matrix4x4_t *in, const float v[4], float out[4])
Definition matrixlib.c:1670
Matrix4x4_FromOriginQuat
void Matrix4x4_FromOriginQuat(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z, double w)
Definition matrixlib.c:1461
Matrix4x4_ToArrayDoubleGL
void Matrix4x4_ToArrayDoubleGL(const matrix4x4_t *in, double out[16])
Definition matrixlib.c:1009
Matrix4x4_Blend
void Matrix4x4_Blend(matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2, double blend)
Definition matrixlib.c:1635
Matrix4x4_FromArrayFloatD3D
void Matrix4x4_FromArrayFloatD3D(matrix4x4_t *out, const float in[16])
Definition matrixlib.c:1282
Matrix4x4_ToArrayFloatGL
void Matrix4x4_ToArrayFloatGL(const matrix4x4_t *in, float out[16])
Definition matrixlib.c:1165
Matrix4x4_CreateTranslate
void Matrix4x4_CreateTranslate(matrix4x4_t *out, double x, double y, double z)
Definition matrixlib.c:584
Matrix4x4_ToArray12FloatD3D
void Matrix4x4_ToArray12FloatD3D(const matrix4x4_t *in, float out[12])
Definition matrixlib.c:1391
Matrix4x4_Transpose
void Matrix4x4_Transpose(matrix4x4_t *out, const matrix4x4_t *in1)
Definition matrixlib.c:122
Matrix4x4_Transform3x3
void Matrix4x4_Transform3x3(const matrix4x4_t *in, const float v[3], float out[3])
Definition matrixlib.c:1685
Matrix4x4_FromArrayFloatGL
void Matrix4x4_FromArrayFloatGL(matrix4x4_t *out, const float in[16])
Definition matrixlib.c:1204
Matrix4x4_FromArrayDoubleGL
void Matrix4x4_FromArrayDoubleGL(matrix4x4_t *out, const double in[16])
Definition matrixlib.c:1048
Matrix4x4_CreateFromQuakeEntity
void Matrix4x4_CreateFromQuakeEntity(matrix4x4_t *out, double x, double y, double z, double pitch, double yaw, double roll, double scale)
Definition matrixlib.c:715
Matrix4x4_CopyTranslateOnly
void Matrix4x4_CopyTranslateOnly(matrix4x4_t *out, const matrix4x4_t *in)
Definition matrixlib.c:44
Matrix4x4_Invert_Full
int Matrix4x4_Invert_Full(matrix4x4_t *out, const matrix4x4_t *in1)
Definition matrixlib.c:145
Matrix4x4_CreateScale3
void Matrix4x4_CreateScale3(matrix4x4_t *out, double x, double y, double z)
Definition matrixlib.c:695
Matrix4x4_ConcatScale3
void Matrix4x4_ConcatScale3(matrix4x4_t *out, double x, double y, double z)
Definition matrixlib.c:1784
Matrix4x4_ToArrayDoubleD3D
void Matrix4x4_ToArrayDoubleD3D(const matrix4x4_t *in, double out[16])
Definition matrixlib.c:1087
Matrix4x4_SetOrigin
void Matrix4x4_SetOrigin(matrix4x4_t *out, double x, double y, double z)
Definition matrixlib.c:1811
Matrix4x4_ConcatScale
void Matrix4x4_ConcatScale(matrix4x4_t *out, double x)
Definition matrixlib.c:1775
Matrix4x4_Normalize3
void Matrix4x4_Normalize3(matrix4x4_t *out, matrix4x4_t *in1)
Definition matrixlib.c:508
Matrix4x4_ToOrigin3Quat4Float
void Matrix4x4_ToOrigin3Quat4Float(const matrix4x4_t *m, float *origin, float *quat)
Definition matrixlib.c:1477
Matrix4x4_FromArray12FloatD3D
void Matrix4x4_FromArray12FloatD3D(matrix4x4_t *out, const float in[12])
Definition matrixlib.c:1422
Matrix4x4_ToArrayFloatD3D
void Matrix4x4_ToArrayFloatD3D(const matrix4x4_t *in, float out[16])
Definition matrixlib.c:1243
Matrix4x4_Reflect
void Matrix4x4_Reflect(matrix4x4_t *out, double normalx, double normaly, double normalz, double dist, double axisscale)
Definition matrixlib.c:535
Matrix4x4_CreateScale
void Matrix4x4_CreateScale(matrix4x4_t *out, double x)
Definition matrixlib.c:675
Matrix4x4_ToArray12FloatGL
void Matrix4x4_ToArray12FloatGL(const matrix4x4_t *in, float out[4][3])
Definition matrixlib.c:1321
Matrix4x4_Interpolate
void Matrix4x4_Interpolate(matrix4x4_t *out, matrix4x4_t *in1, matrix4x4_t *in2, double frac)
Definition matrixlib.c:475
Matrix4x4_Invert_Simple
void Matrix4x4_Invert_Simple(matrix4x4_t *out, const matrix4x4_t *in1)
Definition matrixlib.c:422
Matrix4x4_Abs
void Matrix4x4_Abs(matrix4x4_t *out)
Definition matrixlib.c:1859
Matrix4x4_ConcatTranslate
void Matrix4x4_ConcatTranslate(matrix4x4_t *out, double x, double y, double z)
Definition matrixlib.c:1757
Matrix4x4_ScaleFromMatrix
double Matrix4x4_ScaleFromMatrix(const matrix4x4_t *in)
Definition matrixlib.c:1805
Matrix4x4_FromVectors
void Matrix4x4_FromVectors(matrix4x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
Definition matrixlib.c:970
Matrix4x4_Normalize
void Matrix4x4_Normalize(matrix4x4_t *out, matrix4x4_t *in1)
Definition matrixlib.c:499
Matrix4x4_Accumulate
void Matrix4x4_Accumulate(matrix4x4_t *out, matrix4x4_t *in, double weight)
Definition matrixlib.c:491
Matrix4x4_ToVectors
void Matrix4x4_ToVectors(const matrix4x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
Definition matrixlib.c:939
identitymatrix
const matrix4x4_t identitymatrix
Definition matrixlib.c:9
Matrix4x4_FromArrayDoubleD3D
void Matrix4x4_FromArrayDoubleD3D(matrix4x4_t *out, const double in[4][4])
Definition matrixlib.c:1126
Matrix4x4_ToBonePose7s
void Matrix4x4_ToBonePose7s(const matrix4x4_t *m, float origininvscale, short *pose7s)
Definition matrixlib.c:1614
Matrix4x4_Copy
void Matrix4x4_Copy(matrix4x4_t *out, const matrix4x4_t *in)
Definition matrixlib.c:19
Matrix4x4_CreateRotate
void Matrix4x4_CreateRotate(matrix4x4_t *out, double angle, double x, double y, double z)
Definition matrixlib.c:623
Matrix4x4_ConcatRotate
void Matrix4x4_ConcatRotate(matrix4x4_t *out, double angle, double x, double y, double z)
Definition matrixlib.c:1766
Matrix4x4_FromDoom3Joint
void Matrix4x4_FromDoom3Joint(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z)
Definition matrixlib.c:1582
Matrix4x4_Scale
void Matrix4x4_Scale(matrix4x4_t *out, double rotatescale, double originscale)
Definition matrixlib.c:1837
Matrix4x4_OriginFromMatrix
void Matrix4x4_OriginFromMatrix(const matrix4x4_t *in, float *out)
Definition matrixlib.c:1792
Matrix4x4_CopyRotateOnly
void Matrix4x4_CopyRotateOnly(matrix4x4_t *out, const matrix4x4_t *in)
Definition matrixlib.c:24
cos
float cos(float f)
sqrt
float sqrt(float f)
sin
float sin(float f)
fabs
float fabs(float f)
i
int i
Definition prvm_execprogram.h:2
vec3_t
vec_t vec3_t[3]
Definition qtypes.h:71
r
dp_FragColor r
Definition shader_glsl.h:381
matrix4x4_t
Definition matrixlib.h:10
matrix4x4_t::m
vec_t m[4][4]
Definition matrixlib.h:11
forward
static vec3_t forward
Definition sv_user.c:305
up
static vec3_t up
Definition sv_user.c:305