7 #pragma warning(disable : 4244) // LadyHavoc: MSVC++ 4 x86, double/float
8 #pragma warning(disable : 4305) // LadyHavoc: MSVC++ 6 x86, double/float
11 const matrix4x4_t identitymatrix =
21 void Matrix4x4_Copy (matrix4x4_t *out, const matrix4x4_t *in)
26 void Matrix4x4_CopyRotateOnly (matrix4x4_t *out, const matrix4x4_t *in)
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];
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];
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];
46 void Matrix4x4_CopyTranslateOnly (matrix4x4_t *out, const matrix4x4_t *in)
48 #ifdef MATRIX4x4_OPENGLORIENTATION
52 out->m[3][0] = in->m[0][3];
56 out->m[3][1] = in->m[1][3];
60 out->m[3][2] = in->m[2][3];
69 out->m[0][3] = in->m[0][3];
73 out->m[1][3] = in->m[1][3];
77 out->m[2][3] = in->m[2][3];
85 void Matrix4x4_Concat (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2)
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];
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];
124 void Matrix4x4_Transpose (matrix4x4_t *out, const matrix4x4_t *in1)
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];
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)
151 // note: orientation does not matter, as transpose(invert(transpose(m))) == invert(m), proof:
152 // transpose(invert(transpose(m))) * m
153 // = transpose(invert(transpose(m))) * transpose(transpose(m))
154 // = transpose(transpose(m) * invert(transpose(m)))
155 // = transpose(identity)
158 // this seems to help gcc's common subexpression elimination, and also makes the code look nicer
159 float m00 = in1->m[0][0], m01 = in1->m[0][1], m02 = in1->m[0][2], m03 = in1->m[0][3],
160 m10 = in1->m[1][0], m11 = in1->m[1][1], m12 = in1->m[1][2], m13 = in1->m[1][3],
161 m20 = in1->m[2][0], m21 = in1->m[2][1], m22 = in1->m[2][2], m23 = in1->m[2][3],
162 m30 = in1->m[3][0], m31 = in1->m[3][1], m32 = in1->m[3][2], m33 = in1->m[3][3];
164 // calculate the adjoint
165 out->m[0][0] = (m11*(m22*m33 - m23*m32) - m21*(m12*m33 - m13*m32) + m31*(m12*m23 - m13*m22));
166 out->m[0][1] = -(m01*(m22*m33 - m23*m32) - m21*(m02*m33 - m03*m32) + m31*(m02*m23 - m03*m22));
167 out->m[0][2] = (m01*(m12*m33 - m13*m32) - m11*(m02*m33 - m03*m32) + m31*(m02*m13 - m03*m12));
168 out->m[0][3] = -(m01*(m12*m23 - m13*m22) - m11*(m02*m23 - m03*m22) + m21*(m02*m13 - m03*m12));
169 out->m[1][0] = -(m10*(m22*m33 - m23*m32) - m20*(m12*m33 - m13*m32) + m30*(m12*m23 - m13*m22));
170 out->m[1][1] = (m00*(m22*m33 - m23*m32) - m20*(m02*m33 - m03*m32) + m30*(m02*m23 - m03*m22));
171 out->m[1][2] = -(m00*(m12*m33 - m13*m32) - m10*(m02*m33 - m03*m32) + m30*(m02*m13 - m03*m12));
172 out->m[1][3] = (m00*(m12*m23 - m13*m22) - m10*(m02*m23 - m03*m22) + m20*(m02*m13 - m03*m12));
173 out->m[2][0] = (m10*(m21*m33 - m23*m31) - m20*(m11*m33 - m13*m31) + m30*(m11*m23 - m13*m21));
174 out->m[2][1] = -(m00*(m21*m33 - m23*m31) - m20*(m01*m33 - m03*m31) + m30*(m01*m23 - m03*m21));
175 out->m[2][2] = (m00*(m11*m33 - m13*m31) - m10*(m01*m33 - m03*m31) + m30*(m01*m13 - m03*m11));
176 out->m[2][3] = -(m00*(m11*m23 - m13*m21) - m10*(m01*m23 - m03*m21) + m20*(m01*m13 - m03*m11));
177 out->m[3][0] = -(m10*(m21*m32 - m22*m31) - m20*(m11*m32 - m12*m31) + m30*(m11*m22 - m12*m21));
178 out->m[3][1] = (m00*(m21*m32 - m22*m31) - m20*(m01*m32 - m02*m31) + m30*(m01*m22 - m02*m21));
179 out->m[3][2] = -(m00*(m11*m32 - m12*m31) - m10*(m01*m32 - m02*m31) + m30*(m01*m12 - m02*m11));
180 out->m[3][3] = (m00*(m11*m22 - m12*m21) - m10*(m01*m22 - m02*m21) + m20*(m01*m12 - m02*m11));
182 // calculate the determinant (as inverse == 1/det * adjoint, adjoint * m == identity * det, so this calculates the det)
183 det = m00*out->m[0][0] + m10*out->m[0][1] + m20*out->m[0][2] + m30*out->m[0][3];
187 // multiplications are faster than divisions, usually
190 // manually unrolled loop to multiply all matrix elements by 1/det
191 out->m[0][0] *= det; out->m[0][1] *= det; out->m[0][2] *= det; out->m[0][3] *= det;
192 out->m[1][0] *= det; out->m[1][1] *= det; out->m[1][2] *= det; out->m[1][3] *= det;
193 out->m[2][0] *= det; out->m[2][1] *= det; out->m[2][2] *= det; out->m[2][3] *= det;
194 out->m[3][0] *= det; out->m[3][1] *= det; out->m[3][2] *= det; out->m[3][3] *= det;
199 // Adapted from code contributed to Mesa by David Moore (Mesa 7.6 under SGI Free License B - which is MIT/X11-type)
200 int Matrix4x4_Invert_Full (matrix4x4_t *out, const matrix4x4_t *in1)
206 #ifdef MATRIX4x4_OPENGLORIENTATION
207 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];
208 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];
209 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];
210 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];
211 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];
212 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];
213 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];
214 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];
215 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];
216 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];
217 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];
218 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];
219 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];
220 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];
221 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];
222 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 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];
225 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];
226 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];
227 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];
228 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];
229 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];
230 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];
231 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];
232 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];
233 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];
234 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];
235 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];
236 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];
237 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];
238 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];
239 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];
242 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];
248 for (i = 0;i < 4;i++)
249 for (j = 0;j < 4;j++)
250 out->m[i][j] = temp.m[i][j] * det;
255 int Matrix4x4_Invert_Full (matrix4x4_t *out, const matrix4x4_t *in1)
268 #ifdef MATRIX4x4_OPENGLORIENTATION
269 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];
270 r[0][4] = 1.0; r[0][5] = r[0][6] = r[0][7] = 0.0;
272 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];
273 r[1][5] = 1.0; r[1][4] = r[1][6] = r[1][7] = 0.0;
275 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];
276 r[2][6] = 1.0; r[2][4] = r[2][5] = r[2][7] = 0.0;
278 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];
279 r[3][7] = 1.0; r[3][4] = r[3][5] = r[3][6] = 0.0;
281 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];
282 r[0][4] = 1.0; r[0][5] = r[0][6] = r[0][7] = 0.0;
284 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];
285 r[1][5] = 1.0; r[1][4] = r[1][6] = r[1][7] = 0.0;
287 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];
288 r[2][6] = 1.0; r[2][4] = r[2][5] = r[2][7] = 0.0;
290 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];
291 r[3][7] = 1.0; r[3][4] = r[3][5] = r[3][6] = 0.0;
294 if (fabs (r[3][0]) > fabs (r[2][0])) { temp = r[3]; r[3] = r[2]; r[2] = temp; }
295 if (fabs (r[2][0]) > fabs (r[1][0])) { temp = r[2]; r[2] = r[1]; r[1] = temp; }
296 if (fabs (r[1][0]) > fabs (r[0][0])) { temp = r[1]; r[1] = r[0]; r[0] = temp; }
300 m[1] = r[1][0] / r[0][0];
301 m[2] = r[2][0] / r[0][0];
302 m[3] = r[3][0] / r[0][0];
304 s = r[0][1]; r[1][1] -= m[1] * s; r[2][1] -= m[2] * s; r[3][1] -= m[3] * s;
305 s = r[0][2]; r[1][2] -= m[1] * s; r[2][2] -= m[2] * s; r[3][2] -= m[3] * s;
306 s = r[0][3]; r[1][3] -= m[1] * s; r[2][3] -= m[2] * s; r[3][3] -= m[3] * s;
308 s = r[0][4]; if (s) { r[1][4] -= m[1] * s; r[2][4] -= m[2] * s; r[3][4] -= m[3] * s; }
309 s = r[0][5]; if (s) { r[1][5] -= m[1] * s; r[2][5] -= m[2] * s; r[3][5] -= m[3] * s; }
310 s = r[0][6]; if (s) { r[1][6] -= m[1] * s; r[2][6] -= m[2] * s; r[3][6] -= m[3] * s; }
311 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 if (fabs (r[3][1]) > fabs (r[2][1])) { temp = r[3]; r[3] = r[2]; r[2] = temp; }
314 if (fabs (r[2][1]) > fabs (r[1][1])) { temp = r[2]; r[2] = r[1]; r[1] = temp; }
318 m[2] = r[2][1] / r[1][1];
319 m[3] = r[3][1] / r[1][1];
320 r[2][2] -= m[2] * r[1][2];
321 r[3][2] -= m[3] * r[1][2];
322 r[2][3] -= m[2] * r[1][3];
323 r[3][3] -= m[3] * r[1][3];
325 s = r[1][4]; if (s) { r[2][4] -= m[2] * s; r[3][4] -= m[3] * s; }
326 s = r[1][5]; if (s) { r[2][5] -= m[2] * s; r[3][5] -= m[3] * s; }
327 s = r[1][6]; if (s) { r[2][6] -= m[2] * s; r[3][6] -= m[3] * s; }
328 s = r[1][7]; if (s) { r[2][7] -= m[2] * s; r[3][7] -= m[3] * s; }
330 if (fabs (r[3][2]) > fabs (r[2][2])) { temp = r[3]; r[3] = r[2]; r[2] = temp; }
334 m[3] = r[3][2] / r[2][2];
335 r[3][3] -= m[3] * r[2][3];
336 r[3][4] -= m[3] * r[2][4];
337 r[3][5] -= m[3] * r[2][5];
338 r[3][6] -= m[3] * r[2][6];
339 r[3][7] -= m[3] * r[2][7];
351 r[2][4] = s * (r[2][4] - r[3][4] * m[2]);
352 r[2][5] = s * (r[2][5] - r[3][5] * m[2]);
353 r[2][6] = s * (r[2][6] - r[3][6] * m[2]);
354 r[2][7] = s * (r[2][7] - r[3][7] * m[2]);
357 r[1][4] -= r[3][4] * m[1], r[1][5] -= r[3][5] * m[1];
358 r[1][6] -= r[3][6] * m[1], r[1][7] -= r[3][7] * m[1];
361 r[0][4] -= r[3][4] * m[0], r[0][5] -= r[3][5] * m[0];
362 r[0][6] -= r[3][6] * m[0], r[0][7] -= r[3][7] * m[0];
366 r[1][4] = s * (r[1][4] - r[2][4] * m[1]), r[1][5] = s * (r[1][5] - r[2][5] * m[1]);
367 r[1][6] = s * (r[1][6] - r[2][6] * m[1]), r[1][7] = s * (r[1][7] - r[2][7] * m[1]);
370 r[0][4] -= r[2][4] * m[0], r[0][5] -= r[2][5] * m[0];
371 r[0][6] -= r[2][6] * m[0], r[0][7] -= r[2][7] * m[0];
375 r[0][4] = s * (r[0][4] - r[1][4] * m[0]), r[0][5] = s * (r[0][5] - r[1][5] * m[0]);
376 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 #ifdef MATRIX4x4_OPENGLORIENTATION
379 out->m[0][0] = r[0][4];
380 out->m[0][1] = r[1][4];
381 out->m[0][2] = r[2][4];
382 out->m[0][3] = r[3][4];
383 out->m[1][0] = r[0][5];
384 out->m[1][1] = r[1][5];
385 out->m[1][2] = r[2][5];
386 out->m[1][3] = r[3][5];
387 out->m[2][0] = r[0][6];
388 out->m[2][1] = r[1][6];
389 out->m[2][2] = r[2][6];
390 out->m[2][3] = r[3][6];
391 out->m[3][0] = r[0][7];
392 out->m[3][1] = r[1][7];
393 out->m[3][2] = r[2][7];
394 out->m[3][3] = r[3][7];
396 out->m[0][0] = r[0][4];
397 out->m[0][1] = r[0][5];
398 out->m[0][2] = r[0][6];
399 out->m[0][3] = r[0][7];
400 out->m[1][0] = r[1][4];
401 out->m[1][1] = r[1][5];
402 out->m[1][2] = r[1][6];
403 out->m[1][3] = r[1][7];
404 out->m[2][0] = r[2][4];
405 out->m[2][1] = r[2][5];
406 out->m[2][2] = r[2][6];
407 out->m[2][3] = r[2][7];
408 out->m[3][0] = r[3][4];
409 out->m[3][1] = r[3][5];
410 out->m[3][2] = r[3][6];
411 out->m[3][3] = r[3][7];
424 void Matrix4x4_Invert_Simple (matrix4x4_t *out, const matrix4x4_t *in1)
426 // we only support uniform scaling, so assume the first row is enough
427 // (note the lack of sqrt here, because we're trying to undo the scaling,
428 // this means multiplying by the inverse scale twice - squaring it, which
429 // makes the sqrt a waste of time)
431 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 double scale = 3.0 / sqrt
434 (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]
435 + in1->m[1][0] * in1->m[1][0] + in1->m[1][1] * in1->m[1][1] + in1->m[1][2] * in1->m[1][2]
436 + in1->m[2][0] * in1->m[2][0] + in1->m[2][1] * in1->m[2][1] + in1->m[2][2] * in1->m[2][2]);
440 // invert the rotation by transposing and multiplying by the squared
441 // recipricol of the input matrix scale as described above
442 out->m[0][0] = in1->m[0][0] * scale;
443 out->m[0][1] = in1->m[1][0] * scale;
444 out->m[0][2] = in1->m[2][0] * scale;
445 out->m[1][0] = in1->m[0][1] * scale;
446 out->m[1][1] = in1->m[1][1] * scale;
447 out->m[1][2] = in1->m[2][1] * scale;
448 out->m[2][0] = in1->m[0][2] * scale;
449 out->m[2][1] = in1->m[1][2] * scale;
450 out->m[2][2] = in1->m[2][2] * scale;
452 #ifdef MATRIX4x4_OPENGLORIENTATION
453 // invert the translate
454 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]);
455 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]);
456 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 // don't know if there's anything worth doing here
464 // invert the translate
465 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]);
466 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]);
467 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 // don't know if there's anything worth doing here
477 void Matrix4x4_Interpolate (matrix4x4_t *out, matrix4x4_t *in1, matrix4x4_t *in2, double frac)
480 for (i = 0;i < 4;i++)
481 for (j = 0;j < 4;j++)
482 out->m[i][j] = in1->m[i][j] + frac * (in2->m[i][j] - in1->m[i][j]);
485 void Matrix4x4_Clear (matrix4x4_t *out)
488 for (i = 0;i < 4;i++)
489 for (j = 0;j < 4;j++)
493 void Matrix4x4_Accumulate (matrix4x4_t *out, matrix4x4_t *in, double weight)
496 for (i = 0;i < 4;i++)
497 for (j = 0;j < 4;j++)
498 out->m[i][j] += in->m[i][j] * weight;
501 void Matrix4x4_Normalize (matrix4x4_t *out, matrix4x4_t *in1)
503 // scale rotation matrix vectors to a length of 1
504 // note: this is only designed to undo uniform scaling
505 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 Matrix4x4_Scale(out, scale, 1);
510 void Matrix4x4_Normalize3 (matrix4x4_t *out, matrix4x4_t *in1)
514 // scale each rotation matrix vector to a length of 1
515 // intended for use after Matrix4x4_Interpolate or Matrix4x4_Accumulate
517 for (i = 0;i < 3;i++)
519 #ifdef MATRIX4x4_OPENGLORIENTATION
520 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]);
523 out->m[i][0] *= scale;
524 out->m[i][1] *= scale;
525 out->m[i][2] *= scale;
527 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]);
530 out->m[0][i] *= scale;
531 out->m[1][i] *= scale;
532 out->m[2][i] *= scale;
537 void Matrix4x4_Reflect (matrix4x4_t *out, double normalx, double normaly, double normalz, double dist, double axisscale)
546 p2[0] = p[0] * axisscale;
547 p2[1] = p[1] * axisscale;
548 p2[2] = p[2] * axisscale;
550 for (i = 0;i < 4;i++)
552 #ifdef MATRIX4x4_OPENGLORIENTATION
553 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];
554 out->m[i][0] += p2[0] * d;
555 out->m[i][1] += p2[1] * d;
556 out->m[i][2] += p2[2] * d;
558 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];
559 out->m[0][i] += p2[0] * d;
560 out->m[1][i] += p2[1] * d;
561 out->m[2][i] += p2[2] * d;
566 void Matrix4x4_CreateIdentity (matrix4x4_t *out)
586 void Matrix4x4_CreateTranslate (matrix4x4_t *out, double x, double y, double z)
588 #ifdef MATRIX4x4_OPENGLORIENTATION
625 void Matrix4x4_CreateRotate (matrix4x4_t *out, double angle, double x, double y, double z)
631 len = 1.0f / sqrt(len);
636 angle *= (-M_PI / 180.0);
640 #ifdef MATRIX4x4_OPENGLORIENTATION
641 out->m[0][0]=x * x + c * (1 - x * x);
642 out->m[1][0]=x * y * (1 - c) + z * s;
643 out->m[2][0]=z * x * (1 - c) - y * s;
645 out->m[0][1]=x * y * (1 - c) - z * s;
646 out->m[1][1]=y * y + c * (1 - y * y);
647 out->m[2][1]=y * z * (1 - c) + x * s;
649 out->m[0][2]=z * x * (1 - c) + y * s;
650 out->m[1][2]=y * z * (1 - c) - x * s;
651 out->m[2][2]=z * z + c * (1 - z * z);
658 out->m[0][0]=x * x + c * (1 - x * x);
659 out->m[0][1]=x * y * (1 - c) + z * s;
660 out->m[0][2]=z * x * (1 - c) - y * s;
662 out->m[1][0]=x * y * (1 - c) - z * s;
663 out->m[1][1]=y * y + c * (1 - y * y);
664 out->m[1][2]=y * z * (1 - c) + x * s;
666 out->m[2][0]=z * x * (1 - c) + y * s;
667 out->m[2][1]=y * z * (1 - c) - x * s;
668 out->m[2][2]=z * z + c * (1 - z * z);
677 void Matrix4x4_CreateScale (matrix4x4_t *out, double x)
697 void Matrix4x4_CreateScale3 (matrix4x4_t *out, double x, double y, double z)
717 void Matrix4x4_CreateFromQuakeEntity(matrix4x4_t *out, double x, double y, double z, double pitch, double yaw, double roll, double scale)
719 double angle, sr, sp, sy, cr, cp, cy;
723 angle = yaw * (M_PI*2 / 360);
726 angle = pitch * (M_PI*2 / 360);
729 angle = roll * (M_PI*2 / 360);
732 #ifdef MATRIX4x4_OPENGLORIENTATION
733 out->m[0][0] = (cp*cy) * scale;
734 out->m[1][0] = (sr*sp*cy+cr*-sy) * scale;
735 out->m[2][0] = (cr*sp*cy+-sr*-sy) * scale;
737 out->m[0][1] = (cp*sy) * scale;
738 out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
739 out->m[2][1] = (cr*sp*sy+-sr*cy) * scale;
741 out->m[0][2] = (-sp) * scale;
742 out->m[1][2] = (sr*cp) * scale;
743 out->m[2][2] = (cr*cp) * scale;
750 out->m[0][0] = (cp*cy) * scale;
751 out->m[0][1] = (sr*sp*cy+cr*-sy) * scale;
752 out->m[0][2] = (cr*sp*cy+-sr*-sy) * scale;
754 out->m[1][0] = (cp*sy) * scale;
755 out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
756 out->m[1][2] = (cr*sp*sy+-sr*cy) * scale;
758 out->m[2][0] = (-sp) * scale;
759 out->m[2][1] = (sr*cp) * scale;
760 out->m[2][2] = (cr*cp) * scale;
770 angle = yaw * (M_PI*2 / 360);
773 angle = pitch * (M_PI*2 / 360);
776 #ifdef MATRIX4x4_OPENGLORIENTATION
777 out->m[0][0] = (cp*cy) * scale;
778 out->m[1][0] = (-sy) * scale;
779 out->m[2][0] = (sp*cy) * scale;
781 out->m[0][1] = (cp*sy) * scale;
782 out->m[1][1] = (cy) * scale;
783 out->m[2][1] = (sp*sy) * scale;
785 out->m[0][2] = (-sp) * scale;
787 out->m[2][2] = (cp) * scale;
794 out->m[0][0] = (cp*cy) * scale;
795 out->m[0][1] = (-sy) * scale;
796 out->m[0][2] = (sp*cy) * scale;
798 out->m[1][0] = (cp*sy) * scale;
799 out->m[1][1] = (cy) * scale;
800 out->m[1][2] = (sp*sy) * scale;
802 out->m[2][0] = (-sp) * scale;
804 out->m[2][2] = (cp) * scale;
814 angle = yaw * (M_PI*2 / 360);
817 #ifdef MATRIX4x4_OPENGLORIENTATION
818 out->m[0][0] = (cy) * scale;
819 out->m[1][0] = (-sy) * scale;
822 out->m[0][1] = (sy) * scale;
823 out->m[1][1] = (cy) * scale;
828 out->m[2][2] = scale;
835 out->m[0][0] = (cy) * scale;
836 out->m[0][1] = (-sy) * scale;
839 out->m[1][0] = (sy) * scale;
840 out->m[1][1] = (cy) * scale;
845 out->m[2][2] = scale;
855 #ifdef MATRIX4x4_OPENGLORIENTATION
856 out->m[0][0] = scale;
861 out->m[1][1] = scale;
866 out->m[2][2] = scale;
873 out->m[0][0] = scale;
878 out->m[1][1] = scale;
883 out->m[2][2] = scale;
893 void Matrix4x4_QuakeToDuke3D(const matrix4x4_t *in, matrix4x4_t *out, double maxShearAngle)
895 // Sorry - this isn't direct at all. We can't just use an alternative to
896 // Matrix4x4_CreateFromQuakeEntity as in some cases the input for
897 // generating the view matrix is generated externally.
898 vec3_t forward, left, up, angles;
899 double scaleforward, scaleleft, scaleup;
900 #ifdef MATRIX4x4_OPENGLORIENTATION
901 VectorSet(forward, in->m[0][0], in->m[0][1], in->m[0][2]);
902 VectorSet(left, in->m[1][0], in->m[1][1], in->m[1][2]);
903 VectorSet(up, in->m[2][0], in->m[2][1], in->m[2][2]);
905 VectorSet(forward, in->m[0][0], in->m[1][0], in->m[2][0]);
906 VectorSet(left, in->m[0][1], in->m[1][1], in->m[2][1]);
907 VectorSet(up, in->m[0][2], in->m[1][2], in->m[2][2]);
909 scaleforward = VectorNormalizeLength(forward);
910 scaleleft = VectorNormalizeLength(left);
911 scaleup = VectorNormalizeLength(up);
912 AnglesFromVectors(angles, forward, up, false);
913 AngleVectorsDuke3DFLU(angles, forward, left, up, maxShearAngle);
914 VectorScale(forward, scaleforward, forward);
915 VectorScale(left, scaleleft, left);
916 VectorScale(up, scaleup, up);
918 #ifdef MATRIX4x4_OPENGLORIENTATION
919 out->m[0][0] = forward[0];
920 out->m[1][0] = left[0];
921 out->m[2][0] = up[0];
922 out->m[0][1] = forward[1];
923 out->m[1][1] = left[1];
924 out->m[2][1] = up[1];
925 out->m[0][2] = forward[2];
926 out->m[1][2] = left[2];
927 out->m[2][2] = up[2];
929 out->m[0][0] = forward[0];
930 out->m[0][1] = left[0];
931 out->m[0][2] = up[0];
932 out->m[1][0] = forward[1];
933 out->m[1][1] = left[1];
934 out->m[1][2] = up[1];
935 out->m[2][0] = forward[2];
936 out->m[2][1] = left[2];
937 out->m[2][2] = up[2];
941 void Matrix4x4_ToVectors(const matrix4x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
943 #ifdef MATRIX4x4_OPENGLORIENTATION
972 void Matrix4x4_FromVectors(matrix4x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
974 #ifdef MATRIX4x4_OPENGLORIENTATION
975 out->m[0][0] = vx[0];
976 out->m[1][0] = vy[0];
977 out->m[2][0] = vz[0];
979 out->m[0][1] = vx[1];
980 out->m[1][1] = vy[1];
981 out->m[2][1] = vz[1];
983 out->m[0][2] = vx[2];
984 out->m[1][2] = vy[2];
985 out->m[2][2] = vz[2];
992 out->m[0][0] = vx[0];
993 out->m[0][1] = vy[0];
994 out->m[0][2] = vz[0];
996 out->m[1][0] = vx[1];
997 out->m[1][1] = vy[1];
998 out->m[1][2] = vz[1];
1000 out->m[2][0] = vx[2];
1001 out->m[2][1] = vy[2];
1002 out->m[2][2] = vz[2];
1003 out->m[2][3] = t[2];
1004 out->m[3][0] = 0.0f;
1005 out->m[3][1] = 0.0f;
1006 out->m[3][2] = 0.0f;
1007 out->m[3][3] = 1.0f;
1011 void Matrix4x4_ToArrayDoubleGL(const matrix4x4_t *in, double out[16])
1013 #ifdef MATRIX4x4_OPENGLORIENTATION
1014 out[ 0] = in->m[0][0];
1015 out[ 1] = in->m[0][1];
1016 out[ 2] = in->m[0][2];
1017 out[ 3] = in->m[0][3];
1018 out[ 4] = in->m[1][0];
1019 out[ 5] = in->m[1][1];
1020 out[ 6] = in->m[1][2];
1021 out[ 7] = in->m[1][3];
1022 out[ 8] = in->m[2][0];
1023 out[ 9] = in->m[2][1];
1024 out[10] = in->m[2][2];
1025 out[11] = in->m[2][3];
1026 out[12] = in->m[3][0];
1027 out[13] = in->m[3][1];
1028 out[14] = in->m[3][2];
1029 out[15] = in->m[3][3];
1031 out[ 0] = in->m[0][0];
1032 out[ 1] = in->m[1][0];
1033 out[ 2] = in->m[2][0];
1034 out[ 3] = in->m[3][0];
1035 out[ 4] = in->m[0][1];
1036 out[ 5] = in->m[1][1];
1037 out[ 6] = in->m[2][1];
1038 out[ 7] = in->m[3][1];
1039 out[ 8] = in->m[0][2];
1040 out[ 9] = in->m[1][2];
1041 out[10] = in->m[2][2];
1042 out[11] = in->m[3][2];
1043 out[12] = in->m[0][3];
1044 out[13] = in->m[1][3];
1045 out[14] = in->m[2][3];
1046 out[15] = in->m[3][3];
1050 void Matrix4x4_FromArrayDoubleGL (matrix4x4_t *out, const double in[16])
1052 #ifdef MATRIX4x4_OPENGLORIENTATION
1053 out->m[0][0] = in[0];
1054 out->m[0][1] = in[1];
1055 out->m[0][2] = in[2];
1056 out->m[0][3] = in[3];
1057 out->m[1][0] = in[4];
1058 out->m[1][1] = in[5];
1059 out->m[1][2] = in[6];
1060 out->m[1][3] = in[7];
1061 out->m[2][0] = in[8];
1062 out->m[2][1] = in[9];
1063 out->m[2][2] = in[10];
1064 out->m[2][3] = in[11];
1065 out->m[3][0] = in[12];
1066 out->m[3][1] = in[13];
1067 out->m[3][2] = in[14];
1068 out->m[3][3] = in[15];
1070 out->m[0][0] = in[0];
1071 out->m[1][0] = in[1];
1072 out->m[2][0] = in[2];
1073 out->m[3][0] = in[3];
1074 out->m[0][1] = in[4];
1075 out->m[1][1] = in[5];
1076 out->m[2][1] = in[6];
1077 out->m[3][1] = in[7];
1078 out->m[0][2] = in[8];
1079 out->m[1][2] = in[9];
1080 out->m[2][2] = in[10];
1081 out->m[3][2] = in[11];
1082 out->m[0][3] = in[12];
1083 out->m[1][3] = in[13];
1084 out->m[2][3] = in[14];
1085 out->m[3][3] = in[15];
1089 void Matrix4x4_ToArrayDoubleD3D(const matrix4x4_t *in, double out[16])
1091 #ifdef MATRIX4x4_OPENGLORIENTATION
1092 out[ 0] = in->m[0][0];
1093 out[ 1] = in->m[1][0];
1094 out[ 2] = in->m[2][0];
1095 out[ 3] = in->m[3][0];
1096 out[ 4] = in->m[0][1];
1097 out[ 5] = in->m[1][1];
1098 out[ 6] = in->m[2][1];
1099 out[ 7] = in->m[3][1];
1100 out[ 8] = in->m[0][2];
1101 out[ 9] = in->m[1][2];
1102 out[10] = in->m[2][2];
1103 out[11] = in->m[3][2];
1104 out[12] = in->m[0][3];
1105 out[13] = in->m[1][3];
1106 out[14] = in->m[2][3];
1107 out[15] = in->m[3][3];
1109 out[ 0] = in->m[0][0];
1110 out[ 1] = in->m[0][1];
1111 out[ 2] = in->m[0][2];
1112 out[ 3] = in->m[0][3];
1113 out[ 4] = in->m[1][0];
1114 out[ 5] = in->m[1][1];
1115 out[ 6] = in->m[1][2];
1116 out[ 7] = in->m[1][3];
1117 out[ 8] = in->m[2][0];
1118 out[ 9] = in->m[2][1];
1119 out[10] = in->m[2][2];
1120 out[11] = in->m[2][3];
1121 out[12] = in->m[3][0];
1122 out[13] = in->m[3][1];
1123 out[14] = in->m[3][2];
1124 out[15] = in->m[3][3];
1128 void Matrix4x4_FromArrayDoubleD3D (matrix4x4_t *out, const double in[16])
1130 #ifdef MATRIX4x4_OPENGLORIENTATION
1131 out->m[0][0] = in[0];
1132 out->m[1][0] = in[1];
1133 out->m[2][0] = in[2];
1134 out->m[3][0] = in[3];
1135 out->m[0][1] = in[4];
1136 out->m[1][1] = in[5];
1137 out->m[2][1] = in[6];
1138 out->m[3][1] = in[7];
1139 out->m[0][2] = in[8];
1140 out->m[1][2] = in[9];
1141 out->m[2][2] = in[10];
1142 out->m[3][2] = in[11];
1143 out->m[0][3] = in[12];
1144 out->m[1][3] = in[13];
1145 out->m[2][3] = in[14];
1146 out->m[3][3] = in[15];
1148 out->m[0][0] = in[0];
1149 out->m[0][1] = in[1];
1150 out->m[0][2] = in[2];
1151 out->m[0][3] = in[3];
1152 out->m[1][0] = in[4];
1153 out->m[1][1] = in[5];
1154 out->m[1][2] = in[6];
1155 out->m[1][3] = in[7];
1156 out->m[2][0] = in[8];
1157 out->m[2][1] = in[9];
1158 out->m[2][2] = in[10];
1159 out->m[2][3] = in[11];
1160 out->m[3][0] = in[12];
1161 out->m[3][1] = in[13];
1162 out->m[3][2] = in[14];
1163 out->m[3][3] = in[15];
1167 void Matrix4x4_ToArrayFloatGL(const matrix4x4_t *in, float out[16])
1169 #ifdef MATRIX4x4_OPENGLORIENTATION
1170 out[ 0] = in->m[0][0];
1171 out[ 1] = in->m[0][1];
1172 out[ 2] = in->m[0][2];
1173 out[ 3] = in->m[0][3];
1174 out[ 4] = in->m[1][0];
1175 out[ 5] = in->m[1][1];
1176 out[ 6] = in->m[1][2];
1177 out[ 7] = in->m[1][3];
1178 out[ 8] = in->m[2][0];
1179 out[ 9] = in->m[2][1];
1180 out[10] = in->m[2][2];
1181 out[11] = in->m[2][3];
1182 out[12] = in->m[3][0];
1183 out[13] = in->m[3][1];
1184 out[14] = in->m[3][2];
1185 out[15] = in->m[3][3];
1187 out[ 0] = in->m[0][0];
1188 out[ 1] = in->m[1][0];
1189 out[ 2] = in->m[2][0];
1190 out[ 3] = in->m[3][0];
1191 out[ 4] = in->m[0][1];
1192 out[ 5] = in->m[1][1];
1193 out[ 6] = in->m[2][1];
1194 out[ 7] = in->m[3][1];
1195 out[ 8] = in->m[0][2];
1196 out[ 9] = in->m[1][2];
1197 out[10] = in->m[2][2];
1198 out[11] = in->m[3][2];
1199 out[12] = in->m[0][3];
1200 out[13] = in->m[1][3];
1201 out[14] = in->m[2][3];
1202 out[15] = in->m[3][3];
1206 void Matrix4x4_FromArrayFloatGL (matrix4x4_t *out, const float in[16])
1208 #ifdef MATRIX4x4_OPENGLORIENTATION
1209 out->m[0][0] = in[0];
1210 out->m[0][1] = in[1];
1211 out->m[0][2] = in[2];
1212 out->m[0][3] = in[3];
1213 out->m[1][0] = in[4];
1214 out->m[1][1] = in[5];
1215 out->m[1][2] = in[6];
1216 out->m[1][3] = in[7];
1217 out->m[2][0] = in[8];
1218 out->m[2][1] = in[9];
1219 out->m[2][2] = in[10];
1220 out->m[2][3] = in[11];
1221 out->m[3][0] = in[12];
1222 out->m[3][1] = in[13];
1223 out->m[3][2] = in[14];
1224 out->m[3][3] = in[15];
1226 out->m[0][0] = in[0];
1227 out->m[1][0] = in[1];
1228 out->m[2][0] = in[2];
1229 out->m[3][0] = in[3];
1230 out->m[0][1] = in[4];
1231 out->m[1][1] = in[5];
1232 out->m[2][1] = in[6];
1233 out->m[3][1] = in[7];
1234 out->m[0][2] = in[8];
1235 out->m[1][2] = in[9];
1236 out->m[2][2] = in[10];
1237 out->m[3][2] = in[11];
1238 out->m[0][3] = in[12];
1239 out->m[1][3] = in[13];
1240 out->m[2][3] = in[14];
1241 out->m[3][3] = in[15];
1245 void Matrix4x4_ToArrayFloatD3D(const matrix4x4_t *in, float out[16])
1247 #ifdef MATRIX4x4_OPENGLORIENTATION
1248 out[ 0] = in->m[0][0];
1249 out[ 1] = in->m[1][0];
1250 out[ 2] = in->m[2][0];
1251 out[ 3] = in->m[3][0];
1252 out[ 4] = in->m[0][1];
1253 out[ 5] = in->m[1][1];
1254 out[ 6] = in->m[2][1];
1255 out[ 7] = in->m[3][1];
1256 out[ 8] = in->m[0][2];
1257 out[ 9] = in->m[1][2];
1258 out[10] = in->m[2][2];
1259 out[11] = in->m[3][2];
1260 out[12] = in->m[0][3];
1261 out[13] = in->m[1][3];
1262 out[14] = in->m[2][3];
1263 out[15] = in->m[3][3];
1265 out[ 0] = in->m[0][0];
1266 out[ 1] = in->m[0][1];
1267 out[ 2] = in->m[0][2];
1268 out[ 3] = in->m[0][3];
1269 out[ 4] = in->m[1][0];
1270 out[ 5] = in->m[1][1];
1271 out[ 6] = in->m[1][2];
1272 out[ 7] = in->m[1][3];
1273 out[ 8] = in->m[2][0];
1274 out[ 9] = in->m[2][1];
1275 out[10] = in->m[2][2];
1276 out[11] = in->m[2][3];
1277 out[12] = in->m[3][0];
1278 out[13] = in->m[3][1];
1279 out[14] = in->m[3][2];
1280 out[15] = in->m[3][3];
1284 void Matrix4x4_FromArrayFloatD3D (matrix4x4_t *out, const float in[16])
1286 #ifdef MATRIX4x4_OPENGLORIENTATION
1287 out->m[0][0] = in[0];
1288 out->m[1][0] = in[1];
1289 out->m[2][0] = in[2];
1290 out->m[3][0] = in[3];
1291 out->m[0][1] = in[4];
1292 out->m[1][1] = in[5];
1293 out->m[2][1] = in[6];
1294 out->m[3][1] = in[7];
1295 out->m[0][2] = in[8];
1296 out->m[1][2] = in[9];
1297 out->m[2][2] = in[10];
1298 out->m[3][2] = in[11];
1299 out->m[0][3] = in[12];
1300 out->m[1][3] = in[13];
1301 out->m[2][3] = in[14];
1302 out->m[3][3] = in[15];
1304 out->m[0][0] = in[0];
1305 out->m[0][1] = in[1];
1306 out->m[0][2] = in[2];
1307 out->m[0][3] = in[3];
1308 out->m[1][0] = in[4];
1309 out->m[1][1] = in[5];
1310 out->m[1][2] = in[6];
1311 out->m[1][3] = in[7];
1312 out->m[2][0] = in[8];
1313 out->m[2][1] = in[9];
1314 out->m[2][2] = in[10];
1315 out->m[2][3] = in[11];
1316 out->m[3][0] = in[12];
1317 out->m[3][1] = in[13];
1318 out->m[3][2] = in[14];
1319 out->m[3][3] = in[15];
1323 void Matrix4x4_ToArray12FloatGL(const matrix4x4_t *in, float out[12])
1325 #ifdef MATRIX4x4_OPENGLORIENTATION
1326 out[ 0] = in->m[0][0];
1327 out[ 1] = in->m[0][1];
1328 out[ 2] = in->m[0][2];
1329 out[ 3] = in->m[1][0];
1330 out[ 4] = in->m[1][1];
1331 out[ 5] = in->m[1][2];
1332 out[ 6] = in->m[2][0];
1333 out[ 7] = in->m[2][1];
1334 out[ 8] = in->m[2][2];
1335 out[ 9] = in->m[3][0];
1336 out[10] = in->m[3][1];
1337 out[11] = in->m[3][2];
1339 out[ 0] = in->m[0][0];
1340 out[ 1] = in->m[1][0];
1341 out[ 2] = in->m[2][0];
1342 out[ 3] = in->m[0][1];
1343 out[ 4] = in->m[1][1];
1344 out[ 5] = in->m[2][1];
1345 out[ 6] = in->m[0][2];
1346 out[ 7] = in->m[1][2];
1347 out[ 8] = in->m[2][2];
1348 out[ 9] = in->m[0][3];
1349 out[10] = in->m[1][3];
1350 out[11] = in->m[2][3];
1354 void Matrix4x4_FromArray12FloatGL(matrix4x4_t *out, const float in[12])
1356 #ifdef MATRIX4x4_OPENGLORIENTATION
1357 out->m[0][0] = in[0];
1358 out->m[0][1] = in[1];
1359 out->m[0][2] = in[2];
1361 out->m[1][0] = in[3];
1362 out->m[1][1] = in[4];
1363 out->m[1][2] = in[5];
1365 out->m[2][0] = in[6];
1366 out->m[2][1] = in[7];
1367 out->m[2][2] = in[8];
1369 out->m[3][0] = in[9];
1370 out->m[3][1] = in[10];
1371 out->m[3][2] = in[11];
1374 out->m[0][0] = in[0];
1375 out->m[1][0] = in[1];
1376 out->m[2][0] = in[2];
1378 out->m[0][1] = in[3];
1379 out->m[1][1] = in[4];
1380 out->m[2][1] = in[5];
1382 out->m[0][2] = in[6];
1383 out->m[1][2] = in[7];
1384 out->m[2][2] = in[8];
1386 out->m[0][3] = in[9];
1387 out->m[1][3] = in[10];
1388 out->m[2][3] = in[11];
1393 void Matrix4x4_ToArray12FloatD3D(const matrix4x4_t *in, float out[12])
1395 #ifdef MATRIX4x4_OPENGLORIENTATION
1396 out[ 0] = in->m[0][0];
1397 out[ 1] = in->m[1][0];
1398 out[ 2] = in->m[2][0];
1399 out[ 3] = in->m[3][0];
1400 out[ 4] = in->m[0][1];
1401 out[ 5] = in->m[1][1];
1402 out[ 6] = in->m[2][1];
1403 out[ 7] = in->m[3][1];
1404 out[ 8] = in->m[0][2];
1405 out[ 9] = in->m[1][2];
1406 out[10] = in->m[2][2];
1407 out[11] = in->m[3][2];
1409 out[ 0] = in->m[0][0];
1410 out[ 1] = in->m[0][1];
1411 out[ 2] = in->m[0][2];
1412 out[ 3] = in->m[0][3];
1413 out[ 4] = in->m[1][0];
1414 out[ 5] = in->m[1][1];
1415 out[ 6] = in->m[1][2];
1416 out[ 7] = in->m[1][3];
1417 out[ 8] = in->m[2][0];
1418 out[ 9] = in->m[2][1];
1419 out[10] = in->m[2][2];
1420 out[11] = in->m[2][3];
1424 void Matrix4x4_FromArray12FloatD3D(matrix4x4_t *out, const float in[12])
1426 #ifdef MATRIX4x4_OPENGLORIENTATION
1427 out->m[0][0] = in[0];
1428 out->m[1][0] = in[1];
1429 out->m[2][0] = in[2];
1430 out->m[3][0] = in[3];
1431 out->m[0][1] = in[4];
1432 out->m[1][1] = in[5];
1433 out->m[2][1] = in[6];
1434 out->m[3][1] = in[7];
1435 out->m[0][2] = in[8];
1436 out->m[1][2] = in[9];
1437 out->m[2][2] = in[10];
1438 out->m[3][2] = in[11];
1444 out->m[0][0] = in[0];
1445 out->m[0][1] = in[1];
1446 out->m[0][2] = in[2];
1447 out->m[0][3] = in[3];
1448 out->m[1][0] = in[4];
1449 out->m[1][1] = in[5];
1450 out->m[1][2] = in[6];
1451 out->m[1][3] = in[7];
1452 out->m[2][0] = in[8];
1453 out->m[2][1] = in[9];
1454 out->m[2][2] = in[10];
1455 out->m[2][3] = in[11];
1463 void Matrix4x4_FromOriginQuat(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z, double w)
1465 #ifdef MATRIX4x4_OPENGLORIENTATION
1466 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;
1467 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;
1468 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;
1469 m->m[0][3]= 0 ;m->m[1][3]= 0 ;m->m[2][3]= 0 ;m->m[3][3]=1;
1471 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;
1472 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;
1473 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;
1474 m->m[3][0]= 0 ;m->m[3][1]= 0 ;m->m[3][2]= 0 ;m->m[3][3]=1;
1478 // see http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
1479 void Matrix4x4_ToOrigin3Quat4Float(const matrix4x4_t *m, float *origin, float *quat)
1483 quat[3] = sqrt(1.0f + m->m[0][0] + m->m[1][1] + m->m[2][2]) * 0.5f;
1484 s = 0.25f / quat[3];
1485 #ifdef MATRIX4x4_OPENGLORIENTATION
1486 origin[0] = m->m[3][0];
1487 origin[1] = m->m[3][1];
1488 origin[2] = m->m[3][2];
1489 quat[0] = (m->m[1][2] - m->m[2][1]) * s;
1490 quat[1] = (m->m[2][0] - m->m[0][2]) * s;
1491 quat[2] = (m->m[0][1] - m->m[1][0]) * s;
1493 origin[0] = m->m[0][3];
1494 origin[1] = m->m[1][3];
1495 origin[2] = m->m[2][3];
1496 quat[0] = (m->m[2][1] - m->m[1][2]) * s;
1497 quat[1] = (m->m[0][2] - m->m[2][0]) * s;
1498 quat[2] = (m->m[1][0] - m->m[0][1]) * s;
1503 #ifdef MATRIX4x4_OPENGLORIENTATION
1504 float trace = m->m[0][0] + m->m[1][1] + m->m[2][2];
1505 origin[0] = m->m[3][0];
1506 origin[1] = m->m[3][1];
1507 origin[2] = m->m[3][2];
1510 float r = sqrt(1.0f + trace), inv = 0.5f / r;
1511 quat[0] = (m->m[1][2] - m->m[2][1]) * inv;
1512 quat[1] = (m->m[2][0] - m->m[0][2]) * inv;
1513 quat[2] = (m->m[0][1] - m->m[1][0]) * inv;
1516 else if(m->m[0][0] > m->m[1][1] && m->m[0][0] > m->m[2][2])
1518 float r = sqrt(1.0f + m->m[0][0] - m->m[1][1] - m->m[2][2]), inv = 0.5f / r;
1520 quat[1] = (m->m[0][1] + m->m[1][0]) * inv;
1521 quat[2] = (m->m[2][0] + m->m[0][2]) * inv;
1522 quat[3] = (m->m[1][2] - m->m[2][1]) * inv;
1524 else if(m->m[1][1] > m->m[2][2])
1526 float r = sqrt(1.0f + m->m[1][1] - m->m[0][0] - m->m[2][2]), inv = 0.5f / r;
1527 quat[0] = (m->m[0][1] + m->m[1][0]) * inv;
1529 quat[2] = (m->m[1][2] + m->m[2][1]) * inv;
1530 quat[3] = (m->m[2][0] - m->m[0][2]) * inv;
1534 float r = sqrt(1.0f + m->m[2][2] - m->m[0][0] - m->m[1][1]), inv = 0.5f / r;
1535 quat[0] = (m->m[2][0] + m->m[0][2]) * inv;
1536 quat[1] = (m->m[1][2] + m->m[2][1]) * inv;
1538 quat[3] = (m->m[0][1] - m->m[1][0]) * inv;
1541 float trace = m->m[0][0] + m->m[1][1] + m->m[2][2];
1542 origin[0] = m->m[0][3];
1543 origin[1] = m->m[1][3];
1544 origin[2] = m->m[2][3];
1547 float r = sqrt(1.0f + trace), inv = 0.5f / r;
1548 quat[0] = (m->m[2][1] - m->m[1][2]) * inv;
1549 quat[1] = (m->m[0][2] - m->m[2][0]) * inv;
1550 quat[2] = (m->m[1][0] - m->m[0][1]) * inv;
1553 else if(m->m[0][0] > m->m[1][1] && m->m[0][0] > m->m[2][2])
1555 float r = sqrt(1.0f + m->m[0][0] - m->m[1][1] - m->m[2][2]), inv = 0.5f / r;
1557 quat[1] = (m->m[1][0] + m->m[0][1]) * inv;
1558 quat[2] = (m->m[0][2] + m->m[2][0]) * inv;
1559 quat[3] = (m->m[2][1] - m->m[1][2]) * inv;
1561 else if(m->m[1][1] > m->m[2][2])
1563 float r = sqrt(1.0f + m->m[1][1] - m->m[0][0] - m->m[2][2]), inv = 0.5f / r;
1564 quat[0] = (m->m[1][0] + m->m[0][1]) * inv;
1566 quat[2] = (m->m[2][1] + m->m[1][2]) * inv;
1567 quat[3] = (m->m[0][2] - m->m[2][0]) * inv;
1571 float r = sqrt(1.0f + m->m[2][2] - m->m[0][0] - m->m[1][1]), inv = 0.5f / r;
1572 quat[0] = (m->m[0][2] + m->m[2][0]) * inv;
1573 quat[1] = (m->m[2][1] + m->m[1][2]) * inv;
1575 quat[3] = (m->m[1][0] - m->m[0][1]) * inv;
1582 // LadyHavoc: I got this code from:
1583 //http://www.doom3world.org/phpbb2/viewtopic.php?t=2884
1584 void Matrix4x4_FromDoom3Joint(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z)
1586 double w = 1.0f - (x*x+y*y+z*z);
1587 w = w > 0.0f ? -sqrt(w) : 0.0f;
1588 #ifdef MATRIX4x4_OPENGLORIENTATION
1589 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;
1590 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;
1591 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;
1592 m->m[0][3]= 0 ;m->m[1][3]= 0 ;m->m[2][3]= 0 ;m->m[3][3]=1;
1594 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;
1595 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;
1596 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;
1597 m->m[3][0]= 0 ;m->m[3][1]= 0 ;m->m[3][2]= 0 ;m->m[3][3]=1;
1601 void Matrix4x4_FromBonePose7s(matrix4x4_t *m, float originscale, const short *pose7s)
1605 float quatscale = pose7s[6] > 0 ? -1.0f / 32767.0f : 1.0f / 32767.0f;
1606 origin[0] = pose7s[0] * originscale;
1607 origin[1] = pose7s[1] * originscale;
1608 origin[2] = pose7s[2] * originscale;
1609 quat[0] = pose7s[3] * quatscale;
1610 quat[1] = pose7s[4] * quatscale;
1611 quat[2] = pose7s[5] * quatscale;
1612 quat[3] = pose7s[6] * quatscale;
1613 Matrix4x4_FromOriginQuat(m, origin[0], origin[1], origin[2], quat[0], quat[1], quat[2], quat[3]);
1616 void Matrix4x4_ToBonePose7s(const matrix4x4_t *m, float origininvscale, short *pose7s)
1621 Matrix4x4_ToOrigin3Quat4Float(m, origin, quat);
1622 // normalize quaternion so that it is unit length
1623 quatscale = quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3];
1625 quatscale = (quat[3] >= 0 ? -32767.0f : 32767.0f) / sqrt(quatscale);
1626 // use a negative scale on the quat because the above function produces a
1627 // positive quat[3] and canonical quaternions have negative quat[3]
1628 pose7s[0] = origin[0] * origininvscale;
1629 pose7s[1] = origin[1] * origininvscale;
1630 pose7s[2] = origin[2] * origininvscale;
1631 pose7s[3] = quat[0] * quatscale;
1632 pose7s[4] = quat[1] * quatscale;
1633 pose7s[5] = quat[2] * quatscale;
1634 pose7s[6] = quat[3] * quatscale;
1637 void Matrix4x4_Blend (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2, double blend)
1639 double iblend = 1 - blend;
1640 out->m[0][0] = in1->m[0][0] * iblend + in2->m[0][0] * blend;
1641 out->m[0][1] = in1->m[0][1] * iblend + in2->m[0][1] * blend;
1642 out->m[0][2] = in1->m[0][2] * iblend + in2->m[0][2] * blend;
1643 out->m[0][3] = in1->m[0][3] * iblend + in2->m[0][3] * blend;
1644 out->m[1][0] = in1->m[1][0] * iblend + in2->m[1][0] * blend;
1645 out->m[1][1] = in1->m[1][1] * iblend + in2->m[1][1] * blend;
1646 out->m[1][2] = in1->m[1][2] * iblend + in2->m[1][2] * blend;
1647 out->m[1][3] = in1->m[1][3] * iblend + in2->m[1][3] * blend;
1648 out->m[2][0] = in1->m[2][0] * iblend + in2->m[2][0] * blend;
1649 out->m[2][1] = in1->m[2][1] * iblend + in2->m[2][1] * blend;
1650 out->m[2][2] = in1->m[2][2] * iblend + in2->m[2][2] * blend;
1651 out->m[2][3] = in1->m[2][3] * iblend + in2->m[2][3] * blend;
1652 out->m[3][0] = in1->m[3][0] * iblend + in2->m[3][0] * blend;
1653 out->m[3][1] = in1->m[3][1] * iblend + in2->m[3][1] * blend;
1654 out->m[3][2] = in1->m[3][2] * iblend + in2->m[3][2] * blend;
1655 out->m[3][3] = in1->m[3][3] * iblend + in2->m[3][3] * blend;
1659 void Matrix4x4_Transform (const matrix4x4_t *in, const float v[3], float out[3])
1661 #ifdef MATRIX4x4_OPENGLORIENTATION
1662 out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0] + in->m[3][0];
1663 out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1] + in->m[3][1];
1664 out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2] + in->m[3][2];
1666 out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
1667 out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
1668 out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
1672 void Matrix4x4_Transform4 (const matrix4x4_t *in, const float v[4], float out[4])
1674 #ifdef MATRIX4x4_OPENGLORIENTATION
1675 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];
1676 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];
1677 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];
1678 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];
1680 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];
1681 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];
1682 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];
1683 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];
1687 void Matrix4x4_Transform3x3 (const matrix4x4_t *in, const float v[3], float out[3])
1689 #ifdef MATRIX4x4_OPENGLORIENTATION
1690 out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0];
1691 out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1];
1692 out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2];
1694 out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2];
1695 out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2];
1696 out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2];
1700 // transforms a positive distance plane (A*x+B*y+C*z-D=0) through a rotation or translation matrix
1701 void Matrix4x4_TransformPositivePlane(const matrix4x4_t *in, float x, float y, float z, float d, float *o)
1703 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]);
1704 float iscale = 1.0f / scale;
1705 #ifdef MATRIX4x4_OPENGLORIENTATION
1706 o[0] = (x * in->m[0][0] + y * in->m[1][0] + z * in->m[2][0]) * iscale;
1707 o[1] = (x * in->m[0][1] + y * in->m[1][1] + z * in->m[2][1]) * iscale;
1708 o[2] = (x * in->m[0][2] + y * in->m[1][2] + z * in->m[2][2]) * iscale;
1709 o[3] = d * scale + (o[0] * in->m[3][0] + o[1] * in->m[3][1] + o[2] * in->m[3][2]);
1711 o[0] = (x * in->m[0][0] + y * in->m[0][1] + z * in->m[0][2]) * iscale;
1712 o[1] = (x * in->m[1][0] + y * in->m[1][1] + z * in->m[1][2]) * iscale;
1713 o[2] = (x * in->m[2][0] + y * in->m[2][1] + z * in->m[2][2]) * iscale;
1714 o[3] = d * scale + (o[0] * in->m[0][3] + o[1] * in->m[1][3] + o[2] * in->m[2][3]);
1718 // transforms a standard plane (A*x+B*y+C*z+D=0) through a rotation or translation matrix
1719 void Matrix4x4_TransformStandardPlane(const matrix4x4_t *in, float x, float y, float z, float d, float *o)
1721 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]);
1722 float iscale = 1.0f / scale;
1723 #ifdef MATRIX4x4_OPENGLORIENTATION
1724 o[0] = (x * in->m[0][0] + y * in->m[1][0] + z * in->m[2][0]) * iscale;
1725 o[1] = (x * in->m[0][1] + y * in->m[1][1] + z * in->m[2][1]) * iscale;
1726 o[2] = (x * in->m[0][2] + y * in->m[1][2] + z * in->m[2][2]) * iscale;
1727 o[3] = d * scale - (o[0] * in->m[3][0] + o[1] * in->m[3][1] + o[2] * in->m[3][2]);
1729 o[0] = (x * in->m[0][0] + y * in->m[0][1] + z * in->m[0][2]) * iscale;
1730 o[1] = (x * in->m[1][0] + y * in->m[1][1] + z * in->m[1][2]) * iscale;
1731 o[2] = (x * in->m[2][0] + y * in->m[2][1] + z * in->m[2][2]) * iscale;
1732 o[3] = d * scale - (o[0] * in->m[0][3] + o[1] * in->m[1][3] + o[2] * in->m[2][3]);
1737 void Matrix4x4_SimpleUntransform (const matrix4x4_t *in, const float v[3], float out[3])
1740 #ifdef MATRIX4x4_OPENGLORIENTATION
1741 t[0] = v[0] - in->m[3][0];
1742 t[1] = v[1] - in->m[3][1];
1743 t[2] = v[2] - in->m[3][2];
1744 out[0] = t[0] * in->m[0][0] + t[1] * in->m[0][1] + t[2] * in->m[0][2];
1745 out[1] = t[0] * in->m[1][0] + t[1] * in->m[1][1] + t[2] * in->m[1][2];
1746 out[2] = t[0] * in->m[2][0] + t[1] * in->m[2][1] + t[2] * in->m[2][2];
1748 t[0] = v[0] - in->m[0][3];
1749 t[1] = v[1] - in->m[1][3];
1750 t[2] = v[2] - in->m[2][3];
1751 out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
1752 out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
1753 out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
1759 void Matrix4x4_ConcatTranslate (matrix4x4_t *out, double x, double y, double z)
1761 matrix4x4_t base, temp;
1763 Matrix4x4_CreateTranslate(&temp, x, y, z);
1764 Matrix4x4_Concat(out, &base, &temp);
1768 void Matrix4x4_ConcatRotate (matrix4x4_t *out, double angle, double x, double y, double z)
1770 matrix4x4_t base, temp;
1772 Matrix4x4_CreateRotate(&temp, angle, x, y, z);
1773 Matrix4x4_Concat(out, &base, &temp);
1777 void Matrix4x4_ConcatScale (matrix4x4_t *out, double x)
1779 matrix4x4_t base, temp;
1781 Matrix4x4_CreateScale(&temp, x);
1782 Matrix4x4_Concat(out, &base, &temp);
1786 void Matrix4x4_ConcatScale3 (matrix4x4_t *out, double x, double y, double z)
1788 matrix4x4_t base, temp;
1790 Matrix4x4_CreateScale3(&temp, x, y, z);
1791 Matrix4x4_Concat(out, &base, &temp);
1794 void Matrix4x4_OriginFromMatrix (const matrix4x4_t *in, float *out)
1796 #ifdef MATRIX4x4_OPENGLORIENTATION
1797 out[0] = in->m[3][0];
1798 out[1] = in->m[3][1];
1799 out[2] = in->m[3][2];
1801 out[0] = in->m[0][3];
1802 out[1] = in->m[1][3];
1803 out[2] = in->m[2][3];
1807 double Matrix4x4_ScaleFromMatrix (const matrix4x4_t *in)
1809 // we only support uniform scaling, so assume the first row is enough
1810 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]);
1813 void Matrix4x4_SetOrigin (matrix4x4_t *out, double x, double y, double z)
1815 #ifdef MATRIX4x4_OPENGLORIENTATION
1826 void Matrix4x4_AdjustOrigin (matrix4x4_t *out, double x, double y, double z)
1828 #ifdef MATRIX4x4_OPENGLORIENTATION
1839 void Matrix4x4_Scale (matrix4x4_t *out, double rotatescale, double originscale)
1841 out->m[0][0] *= rotatescale;
1842 out->m[0][1] *= rotatescale;
1843 out->m[0][2] *= rotatescale;
1844 out->m[1][0] *= rotatescale;
1845 out->m[1][1] *= rotatescale;
1846 out->m[1][2] *= rotatescale;
1847 out->m[2][0] *= rotatescale;
1848 out->m[2][1] *= rotatescale;
1849 out->m[2][2] *= rotatescale;
1850 #ifdef MATRIX4x4_OPENGLORIENTATION
1851 out->m[3][0] *= originscale;
1852 out->m[3][1] *= originscale;
1853 out->m[3][2] *= originscale;
1855 out->m[0][3] *= originscale;
1856 out->m[1][3] *= originscale;
1857 out->m[2][3] *= originscale;
1861 void Matrix4x4_Abs (matrix4x4_t *out)
1863 out->m[0][0] = fabs(out->m[0][0]);
1864 out->m[0][1] = fabs(out->m[0][1]);
1865 out->m[0][2] = fabs(out->m[0][2]);
1866 out->m[1][0] = fabs(out->m[1][0]);
1867 out->m[1][1] = fabs(out->m[1][1]);
1868 out->m[1][2] = fabs(out->m[1][2]);
1869 out->m[2][0] = fabs(out->m[2][0]);
1870 out->m[2][1] = fabs(out->m[2][1]);
1871 out->m[2][2] = fabs(out->m[2][2]);