7 #pragma warning(disable : 4244) // LordHavoc: MSVC++ 4 x86, double/float
8 #pragma warning(disable : 4305) // LordHavoc: 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 int Matrix4x4_Invert_Full (matrix4x4_t *out, const matrix4x4_t *in1)
152 #ifdef MATRIX4x4_OPENGLORIENTATION
153 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];
154 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];
155 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];
156 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];
157 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];
158 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];
159 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];
160 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];
161 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];
162 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];
163 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];
164 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];
165 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];
166 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];
167 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];
168 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];
170 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];
171 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];
172 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];
173 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];
174 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];
175 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];
176 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];
177 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];
178 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];
179 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];
180 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];
181 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];
182 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];
183 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];
184 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];
185 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];
188 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];
194 for (i = 0;i < 4;i++)
195 for (j = 0;j < 4;j++)
196 out->m[i][j] = temp.m[i][j] * det;
201 // Adapted from code contributed to Mesa by David Moore (Mesa 7.6 under SGI Free License B - which is MIT/X11-type)
202 // number of multiplications reduced by divVerent, yet to be benchmarked
203 int Matrix4x4_Invert_Full (matrix4x4_t *out, const matrix4x4_t *in1)
209 #ifdef MATRIX4x4_OPENGLORIENTATION
210 temp.m[0][0] = in1->m[1][1]*(in1->m[2][2]*in1->m[3][3] - in1->m[2][3]*in1->m[3][2]) - in1->m[2][1]*(in1->m[1][2]*in1->m[3][3] - in1->m[1][3]*in1->m[3][2]) + in1->m[3][1]*(in1->m[1][2]*in1->m[2][3] - in1->m[1][3]*in1->m[2][2]);
211 temp.m[1][0] = -in1->m[1][0]*(in1->m[2][2]*in1->m[3][3] - in1->m[2][3]*in1->m[3][2]) + in1->m[2][0]*(in1->m[1][2]*in1->m[3][3] - in1->m[1][3]*in1->m[3][2]) - in1->m[3][0]*(in1->m[1][2]*in1->m[2][3] - in1->m[1][3]*in1->m[2][2]);
212 temp.m[2][0] = in1->m[1][0]*(in1->m[2][1]*in1->m[3][3] - in1->m[2][3]*in1->m[3][1]) - in1->m[2][0]*(in1->m[1][1]*in1->m[3][3] - in1->m[1][3]*in1->m[3][1]) + in1->m[3][0]*(in1->m[1][1]*in1->m[2][3] - in1->m[1][3]*in1->m[2][1]);
213 temp.m[3][0] = -in1->m[1][0]*(in1->m[2][1]*in1->m[3][2] - in1->m[2][2]*in1->m[3][1]) + in1->m[2][0]*(in1->m[1][1]*in1->m[3][2] - in1->m[1][2]*in1->m[3][1]) - in1->m[3][0]*(in1->m[1][1]*in1->m[2][2] - in1->m[1][2]*in1->m[2][1]);
214 temp.m[0][1] = -in1->m[0][1]*(in1->m[2][2]*in1->m[3][3] - in1->m[2][3]*in1->m[3][2]) + in1->m[2][1]*(in1->m[0][2]*in1->m[3][3] - in1->m[0][3]*in1->m[3][2]) - in1->m[3][1]*(in1->m[0][2]*in1->m[2][3] - in1->m[0][3]*in1->m[2][2]);
215 temp.m[1][1] = in1->m[0][0]*(in1->m[2][2]*in1->m[3][3] - in1->m[2][3]*in1->m[3][2]) - in1->m[2][0]*(in1->m[0][2]*in1->m[3][3] - in1->m[0][3]*in1->m[3][2]) + in1->m[3][0]*(in1->m[0][2]*in1->m[2][3] - in1->m[0][3]*in1->m[2][2]);
216 temp.m[2][1] = -in1->m[0][0]*(in1->m[2][1]*in1->m[3][3] - in1->m[2][3]*in1->m[3][1]) + in1->m[2][0]*(in1->m[0][1]*in1->m[3][3] - in1->m[0][3]*in1->m[3][1]) - in1->m[3][0]*(in1->m[0][1]*in1->m[2][3] - in1->m[0][3]*in1->m[2][1]);
217 temp.m[3][1] = in1->m[0][0]*(in1->m[2][1]*in1->m[3][2] - in1->m[2][2]*in1->m[3][1]) - in1->m[2][0]*(in1->m[0][1]*in1->m[3][2] - in1->m[0][2]*in1->m[3][1]) + in1->m[3][0]*(in1->m[0][1]*in1->m[2][2] - in1->m[0][2]*in1->m[2][1]);
218 temp.m[0][2] = in1->m[0][1]*(in1->m[1][2]*in1->m[3][3] - in1->m[1][3]*in1->m[3][2]) - in1->m[1][1]*(in1->m[0][2]*in1->m[3][3] - in1->m[0][3]*in1->m[3][2]) + in1->m[3][1]*(in1->m[0][2]*in1->m[1][3] - in1->m[0][3]*in1->m[1][2]);
219 temp.m[1][2] = -in1->m[0][0]*(in1->m[1][2]*in1->m[3][3] - in1->m[1][3]*in1->m[3][2]) + in1->m[1][0]*(in1->m[0][2]*in1->m[3][3] - in1->m[0][3]*in1->m[3][2]) - in1->m[3][0]*(in1->m[0][2]*in1->m[1][3] - in1->m[0][3]*in1->m[1][2]);
220 temp.m[2][2] = in1->m[0][0]*(in1->m[1][1]*in1->m[3][3] - in1->m[1][3]*in1->m[3][1]) - in1->m[1][0]*(in1->m[0][1]*in1->m[3][3] - in1->m[0][3]*in1->m[3][1]) + in1->m[3][0]*(in1->m[0][1]*in1->m[1][3] - in1->m[0][3]*in1->m[1][1]);
221 temp.m[3][2] = -in1->m[0][0]*(in1->m[1][1]*in1->m[3][2] - in1->m[1][2]*in1->m[3][1]) + in1->m[1][0]*(in1->m[0][1]*in1->m[3][2] - in1->m[0][2]*in1->m[3][1]) - in1->m[3][0]*(in1->m[0][1]*in1->m[1][2] - in1->m[0][2]*in1->m[1][1]);
222 temp.m[0][3] = -in1->m[0][1]*(in1->m[1][2]*in1->m[2][3] - in1->m[1][3]*in1->m[2][2]) + in1->m[1][1]*(in1->m[0][2]*in1->m[2][3] - in1->m[0][3]*in1->m[2][2]) - in1->m[2][1]*(in1->m[0][2]*in1->m[1][3] - in1->m[0][3]*in1->m[1][2]);
223 temp.m[1][3] = in1->m[0][0]*(in1->m[1][2]*in1->m[2][3] - in1->m[1][3]*in1->m[2][2]) - in1->m[1][0]*(in1->m[0][2]*in1->m[2][3] - in1->m[0][3]*in1->m[2][2]) + in1->m[2][0]*(in1->m[0][2]*in1->m[1][3] - in1->m[0][3]*in1->m[1][2]);
224 temp.m[2][3] = -in1->m[0][0]*(in1->m[1][1]*in1->m[2][3] - in1->m[1][3]*in1->m[2][1]) + in1->m[1][0]*(in1->m[0][1]*in1->m[2][3] - in1->m[0][3]*in1->m[2][1]) - in1->m[2][0]*(in1->m[0][1]*in1->m[1][3] - in1->m[0][3]*in1->m[1][1]);
225 temp.m[3][3] = in1->m[0][0]*(in1->m[1][1]*in1->m[2][2] - in1->m[1][2]*in1->m[2][1]) - in1->m[1][0]*(in1->m[0][1]*in1->m[2][2] - in1->m[0][2]*in1->m[2][1]) + in1->m[2][0]*(in1->m[0][1]*in1->m[1][2] - in1->m[0][2]*in1->m[1][1]);
227 temp.m[0][0] = in1->m[1][1]*(in1->m[2][2]*in1->m[3][3] - in1->m[3][2]*in1->m[2][3]) - in1->m[1][2]*(in1->m[2][1]*in1->m[3][3] - in1->m[3][1]*in1->m[2][3]) + in1->m[1][3]*(in1->m[2][1]*in1->m[3][2] - in1->m[3][1]*in1->m[2][2]);
228 temp.m[0][1] = -in1->m[0][1]*(in1->m[2][2]*in1->m[3][3] - in1->m[3][2]*in1->m[2][3]) + in1->m[0][2]*(in1->m[2][1]*in1->m[3][3] - in1->m[3][1]*in1->m[2][3]) - in1->m[0][3]*(in1->m[2][1]*in1->m[3][2] - in1->m[3][1]*in1->m[2][2]);
229 temp.m[0][2] = in1->m[0][1]*(in1->m[1][2]*in1->m[3][3] - in1->m[3][2]*in1->m[1][3]) - in1->m[0][2]*(in1->m[1][1]*in1->m[3][3] - in1->m[3][1]*in1->m[1][3]) + in1->m[0][3]*(in1->m[1][1]*in1->m[3][2] - in1->m[3][1]*in1->m[1][2]);
230 temp.m[0][3] = -in1->m[0][1]*(in1->m[1][2]*in1->m[2][3] - in1->m[2][2]*in1->m[1][3]) + in1->m[0][2]*(in1->m[1][1]*in1->m[2][3] - in1->m[2][1]*in1->m[1][3]) - in1->m[0][3]*(in1->m[1][1]*in1->m[2][2] - in1->m[2][1]*in1->m[1][2]);
231 temp.m[1][0] = -in1->m[1][0]*(in1->m[2][2]*in1->m[3][3] - in1->m[3][2]*in1->m[2][3]) + in1->m[1][2]*(in1->m[2][0]*in1->m[3][3] - in1->m[3][0]*in1->m[2][3]) - in1->m[1][3]*(in1->m[2][0]*in1->m[3][2] - in1->m[3][0]*in1->m[2][2]);
232 temp.m[1][1] = in1->m[0][0]*(in1->m[2][2]*in1->m[3][3] - in1->m[3][2]*in1->m[2][3]) - in1->m[0][2]*(in1->m[2][0]*in1->m[3][3] - in1->m[3][0]*in1->m[2][3]) + in1->m[0][3]*(in1->m[2][0]*in1->m[3][2] - in1->m[3][0]*in1->m[2][2]);
233 temp.m[1][2] = -in1->m[0][0]*(in1->m[1][2]*in1->m[3][3] - in1->m[3][2]*in1->m[1][3]) + in1->m[0][2]*(in1->m[1][0]*in1->m[3][3] - in1->m[3][0]*in1->m[1][3]) - in1->m[0][3]*(in1->m[1][0]*in1->m[3][2] - in1->m[3][0]*in1->m[1][2]);
234 temp.m[1][3] = in1->m[0][0]*(in1->m[1][2]*in1->m[2][3] - in1->m[2][2]*in1->m[1][3]) - in1->m[0][2]*(in1->m[1][0]*in1->m[2][3] - in1->m[2][0]*in1->m[1][3]) + in1->m[0][3]*(in1->m[1][0]*in1->m[2][2] - in1->m[2][0]*in1->m[1][2]);
235 temp.m[2][0] = in1->m[1][0]*(in1->m[2][1]*in1->m[3][3] - in1->m[3][1]*in1->m[2][3]) - in1->m[1][1]*(in1->m[2][0]*in1->m[3][3] - in1->m[3][0]*in1->m[2][3]) + in1->m[1][3]*(in1->m[2][0]*in1->m[3][1] - in1->m[3][0]*in1->m[2][1]);
236 temp.m[2][1] = -in1->m[0][0]*(in1->m[2][1]*in1->m[3][3] - in1->m[3][1]*in1->m[2][3]) + in1->m[0][1]*(in1->m[2][0]*in1->m[3][3] - in1->m[3][0]*in1->m[2][3]) - in1->m[0][3]*(in1->m[2][0]*in1->m[3][1] - in1->m[3][0]*in1->m[2][1]);
237 temp.m[2][2] = in1->m[0][0]*(in1->m[1][1]*in1->m[3][3] - in1->m[3][1]*in1->m[1][3]) - in1->m[0][1]*(in1->m[1][0]*in1->m[3][3] - in1->m[3][0]*in1->m[1][3]) + in1->m[0][3]*(in1->m[1][0]*in1->m[3][1] - in1->m[3][0]*in1->m[1][1]);
238 temp.m[2][3] = -in1->m[0][0]*(in1->m[1][1]*in1->m[2][3] - in1->m[2][1]*in1->m[1][3]) + in1->m[0][1]*(in1->m[1][0]*in1->m[2][3] - in1->m[2][0]*in1->m[1][3]) - in1->m[0][3]*(in1->m[1][0]*in1->m[2][1] - in1->m[2][0]*in1->m[1][1]);
239 temp.m[3][0] = -in1->m[1][0]*(in1->m[2][1]*in1->m[3][2] - in1->m[3][1]*in1->m[2][2]) + in1->m[1][1]*(in1->m[2][0]*in1->m[3][2] - in1->m[3][0]*in1->m[2][2]) - in1->m[1][2]*(in1->m[2][0]*in1->m[3][1] - in1->m[3][0]*in1->m[2][1]);
240 temp.m[3][1] = in1->m[0][0]*(in1->m[2][1]*in1->m[3][2] - in1->m[3][1]*in1->m[2][2]) - in1->m[0][1]*(in1->m[2][0]*in1->m[3][2] - in1->m[3][0]*in1->m[2][2]) + in1->m[0][2]*(in1->m[2][0]*in1->m[3][1] - in1->m[3][0]*in1->m[2][1]);
241 temp.m[3][2] = -in1->m[0][0]*(in1->m[1][1]*in1->m[3][2] - in1->m[3][1]*in1->m[1][2]) + in1->m[0][1]*(in1->m[1][0]*in1->m[3][2] - in1->m[3][0]*in1->m[1][2]) - in1->m[0][2]*(in1->m[1][0]*in1->m[3][1] - in1->m[3][0]*in1->m[1][1]);
242 temp.m[3][3] = in1->m[0][0]*(in1->m[1][1]*in1->m[2][2] - in1->m[2][1]*in1->m[1][2]) - in1->m[0][1]*(in1->m[1][0]*in1->m[2][2] - in1->m[2][0]*in1->m[1][2]) + in1->m[0][2]*(in1->m[1][0]*in1->m[2][1] - in1->m[2][0]*in1->m[1][1]);
245 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];
251 for (i = 0;i < 4;i++)
252 for (j = 0;j < 4;j++)
253 out->m[i][j] = temp.m[i][j] * det;
258 int Matrix4x4_Invert_Full (matrix4x4_t *out, const matrix4x4_t *in1)
271 #ifdef MATRIX4x4_OPENGLORIENTATION
272 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];
273 r[0][4] = 1.0; r[0][5] = r[0][6] = r[0][7] = 0.0;
275 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];
276 r[1][5] = 1.0; r[1][4] = r[1][6] = r[1][7] = 0.0;
278 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];
279 r[2][6] = 1.0; r[2][4] = r[2][5] = r[2][7] = 0.0;
281 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];
282 r[3][7] = 1.0; r[3][4] = r[3][5] = r[3][6] = 0.0;
284 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];
285 r[0][4] = 1.0; r[0][5] = r[0][6] = r[0][7] = 0.0;
287 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];
288 r[1][5] = 1.0; r[1][4] = r[1][6] = r[1][7] = 0.0;
290 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];
291 r[2][6] = 1.0; r[2][4] = r[2][5] = r[2][7] = 0.0;
293 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];
294 r[3][7] = 1.0; r[3][4] = r[3][5] = r[3][6] = 0.0;
297 if (fabs (r[3][0]) > fabs (r[2][0])) { temp = r[3]; r[3] = r[2]; r[2] = temp; }
298 if (fabs (r[2][0]) > fabs (r[1][0])) { temp = r[2]; r[2] = r[1]; r[1] = temp; }
299 if (fabs (r[1][0]) > fabs (r[0][0])) { temp = r[1]; r[1] = r[0]; r[0] = temp; }
303 m[1] = r[1][0] / r[0][0];
304 m[2] = r[2][0] / r[0][0];
305 m[3] = r[3][0] / r[0][0];
307 s = r[0][1]; r[1][1] -= m[1] * s; r[2][1] -= m[2] * s; r[3][1] -= m[3] * s;
308 s = r[0][2]; r[1][2] -= m[1] * s; r[2][2] -= m[2] * s; r[3][2] -= m[3] * s;
309 s = r[0][3]; r[1][3] -= m[1] * s; r[2][3] -= m[2] * s; r[3][3] -= m[3] * s;
311 s = r[0][4]; if (s) { r[1][4] -= m[1] * s; r[2][4] -= m[2] * s; r[3][4] -= m[3] * s; }
312 s = r[0][5]; if (s) { r[1][5] -= m[1] * s; r[2][5] -= m[2] * s; r[3][5] -= m[3] * s; }
313 s = r[0][6]; if (s) { r[1][6] -= m[1] * s; r[2][6] -= m[2] * s; r[3][6] -= m[3] * s; }
314 s = r[0][7]; if (s) { r[1][7] -= m[1] * s; r[2][7] -= m[2] * s; r[3][7] -= m[3] * s; }
316 if (fabs (r[3][1]) > fabs (r[2][1])) { temp = r[3]; r[3] = r[2]; r[2] = temp; }
317 if (fabs (r[2][1]) > fabs (r[1][1])) { temp = r[2]; r[2] = r[1]; r[1] = temp; }
321 m[2] = r[2][1] / r[1][1];
322 m[3] = r[3][1] / r[1][1];
323 r[2][2] -= m[2] * r[1][2];
324 r[3][2] -= m[3] * r[1][2];
325 r[2][3] -= m[2] * r[1][3];
326 r[3][3] -= m[3] * r[1][3];
328 s = r[1][4]; if (s) { r[2][4] -= m[2] * s; r[3][4] -= m[3] * s; }
329 s = r[1][5]; if (s) { r[2][5] -= m[2] * s; r[3][5] -= m[3] * s; }
330 s = r[1][6]; if (s) { r[2][6] -= m[2] * s; r[3][6] -= m[3] * s; }
331 s = r[1][7]; if (s) { r[2][7] -= m[2] * s; r[3][7] -= m[3] * s; }
333 if (fabs (r[3][2]) > fabs (r[2][2])) { temp = r[3]; r[3] = r[2]; r[2] = temp; }
337 m[3] = r[3][2] / r[2][2];
338 r[3][3] -= m[3] * r[2][3];
339 r[3][4] -= m[3] * r[2][4];
340 r[3][5] -= m[3] * r[2][5];
341 r[3][6] -= m[3] * r[2][6];
342 r[3][7] -= m[3] * r[2][7];
354 r[2][4] = s * (r[2][4] - r[3][4] * m[2]);
355 r[2][5] = s * (r[2][5] - r[3][5] * m[2]);
356 r[2][6] = s * (r[2][6] - r[3][6] * m[2]);
357 r[2][7] = s * (r[2][7] - r[3][7] * m[2]);
360 r[1][4] -= r[3][4] * m[1], r[1][5] -= r[3][5] * m[1];
361 r[1][6] -= r[3][6] * m[1], r[1][7] -= r[3][7] * m[1];
364 r[0][4] -= r[3][4] * m[0], r[0][5] -= r[3][5] * m[0];
365 r[0][6] -= r[3][6] * m[0], r[0][7] -= r[3][7] * m[0];
369 r[1][4] = s * (r[1][4] - r[2][4] * m[1]), r[1][5] = s * (r[1][5] - r[2][5] * m[1]);
370 r[1][6] = s * (r[1][6] - r[2][6] * m[1]), r[1][7] = s * (r[1][7] - r[2][7] * m[1]);
373 r[0][4] -= r[2][4] * m[0], r[0][5] -= r[2][5] * m[0];
374 r[0][6] -= r[2][6] * m[0], r[0][7] -= r[2][7] * m[0];
378 r[0][4] = s * (r[0][4] - r[1][4] * m[0]), r[0][5] = s * (r[0][5] - r[1][5] * m[0]);
379 r[0][6] = s * (r[0][6] - r[1][6] * m[0]), r[0][7] = s * (r[0][7] - r[1][7] * m[0]);
381 #ifdef MATRIX4x4_OPENGLORIENTATION
382 out->m[0][0] = r[0][4];
383 out->m[0][1] = r[1][4];
384 out->m[0][2] = r[2][4];
385 out->m[0][3] = r[3][4];
386 out->m[1][0] = r[0][5];
387 out->m[1][1] = r[1][5];
388 out->m[1][2] = r[2][5];
389 out->m[1][3] = r[3][5];
390 out->m[2][0] = r[0][6];
391 out->m[2][1] = r[1][6];
392 out->m[2][2] = r[2][6];
393 out->m[2][3] = r[3][6];
394 out->m[3][0] = r[0][7];
395 out->m[3][1] = r[1][7];
396 out->m[3][2] = r[2][7];
397 out->m[3][3] = r[3][7];
399 out->m[0][0] = r[0][4];
400 out->m[0][1] = r[0][5];
401 out->m[0][2] = r[0][6];
402 out->m[0][3] = r[0][7];
403 out->m[1][0] = r[1][4];
404 out->m[1][1] = r[1][5];
405 out->m[1][2] = r[1][6];
406 out->m[1][3] = r[1][7];
407 out->m[2][0] = r[2][4];
408 out->m[2][1] = r[2][5];
409 out->m[2][2] = r[2][6];
410 out->m[2][3] = r[2][7];
411 out->m[3][0] = r[3][4];
412 out->m[3][1] = r[3][5];
413 out->m[3][2] = r[3][6];
414 out->m[3][3] = r[3][7];
427 void Matrix4x4_Invert_Simple (matrix4x4_t *out, const matrix4x4_t *in1)
429 // we only support uniform scaling, so assume the first row is enough
430 // (note the lack of sqrt here, because we're trying to undo the scaling,
431 // this means multiplying by the inverse scale twice - squaring it, which
432 // makes the sqrt a waste of time)
434 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]);
436 double scale = 3.0 / sqrt
437 (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]
438 + in1->m[1][0] * in1->m[1][0] + in1->m[1][1] * in1->m[1][1] + in1->m[1][2] * in1->m[1][2]
439 + in1->m[2][0] * in1->m[2][0] + in1->m[2][1] * in1->m[2][1] + in1->m[2][2] * in1->m[2][2]);
443 // invert the rotation by transposing and multiplying by the squared
444 // recipricol of the input matrix scale as described above
445 out->m[0][0] = in1->m[0][0] * scale;
446 out->m[0][1] = in1->m[1][0] * scale;
447 out->m[0][2] = in1->m[2][0] * scale;
448 out->m[1][0] = in1->m[0][1] * scale;
449 out->m[1][1] = in1->m[1][1] * scale;
450 out->m[1][2] = in1->m[2][1] * scale;
451 out->m[2][0] = in1->m[0][2] * scale;
452 out->m[2][1] = in1->m[1][2] * scale;
453 out->m[2][2] = in1->m[2][2] * scale;
455 #ifdef MATRIX4x4_OPENGLORIENTATION
456 // invert the translate
457 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]);
458 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]);
459 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]);
461 // don't know if there's anything worth doing here
467 // invert the translate
468 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]);
469 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]);
470 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]);
472 // don't know if there's anything worth doing here
480 void Matrix4x4_Interpolate (matrix4x4_t *out, matrix4x4_t *in1, matrix4x4_t *in2, double frac)
483 for (i = 0;i < 4;i++)
484 for (j = 0;j < 4;j++)
485 out->m[i][j] = in1->m[i][j] + frac * (in2->m[i][j] - in1->m[i][j]);
488 void Matrix4x4_Clear (matrix4x4_t *out)
491 for (i = 0;i < 4;i++)
492 for (j = 0;j < 4;j++)
496 void Matrix4x4_Accumulate (matrix4x4_t *out, matrix4x4_t *in, double weight)
499 for (i = 0;i < 4;i++)
500 for (j = 0;j < 4;j++)
501 out->m[i][j] += in->m[i][j] * weight;
504 void Matrix4x4_Normalize (matrix4x4_t *out, matrix4x4_t *in1)
506 // scale rotation matrix vectors to a length of 1
507 // note: this is only designed to undo uniform scaling
508 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]);
510 Matrix4x4_Scale(out, scale, 1);
513 void Matrix4x4_Normalize3 (matrix4x4_t *out, matrix4x4_t *in1)
517 // scale each rotation matrix vector to a length of 1
518 // intended for use after Matrix4x4_Interpolate or Matrix4x4_Accumulate
520 for (i = 0;i < 3;i++)
522 #ifdef MATRIX4x4_OPENGLORIENTATION
523 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]);
526 out->m[i][0] *= scale;
527 out->m[i][1] *= scale;
528 out->m[i][2] *= scale;
530 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]);
533 out->m[0][i] *= scale;
534 out->m[1][i] *= scale;
535 out->m[2][i] *= scale;
540 void Matrix4x4_Reflect (matrix4x4_t *out, double normalx, double normaly, double normalz, double dist, double axisscale)
549 p2[0] = p[0] * axisscale;
550 p2[1] = p[1] * axisscale;
551 p2[2] = p[2] * axisscale;
553 for (i = 0;i < 4;i++)
555 #ifdef MATRIX4x4_OPENGLORIENTATION
556 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];
557 out->m[i][0] += p2[0] * d;
558 out->m[i][1] += p2[1] * d;
559 out->m[i][2] += p2[2] * d;
561 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];
562 out->m[0][i] += p2[0] * d;
563 out->m[1][i] += p2[1] * d;
564 out->m[2][i] += p2[2] * d;
569 void Matrix4x4_CreateIdentity (matrix4x4_t *out)
589 void Matrix4x4_CreateTranslate (matrix4x4_t *out, double x, double y, double z)
591 #ifdef MATRIX4x4_OPENGLORIENTATION
628 void Matrix4x4_CreateRotate (matrix4x4_t *out, double angle, double x, double y, double z)
634 len = 1.0f / sqrt(len);
639 angle *= (-M_PI / 180.0);
643 #ifdef MATRIX4x4_OPENGLORIENTATION
644 out->m[0][0]=x * x + c * (1 - x * x);
645 out->m[1][0]=x * y * (1 - c) + z * s;
646 out->m[2][0]=z * x * (1 - c) - y * s;
648 out->m[0][1]=x * y * (1 - c) - z * s;
649 out->m[1][1]=y * y + c * (1 - y * y);
650 out->m[2][1]=y * z * (1 - c) + x * s;
652 out->m[0][2]=z * x * (1 - c) + y * s;
653 out->m[1][2]=y * z * (1 - c) - x * s;
654 out->m[2][2]=z * z + c * (1 - z * z);
661 out->m[0][0]=x * x + c * (1 - x * x);
662 out->m[0][1]=x * y * (1 - c) + z * s;
663 out->m[0][2]=z * x * (1 - c) - y * s;
665 out->m[1][0]=x * y * (1 - c) - z * s;
666 out->m[1][1]=y * y + c * (1 - y * y);
667 out->m[1][2]=y * z * (1 - c) + x * s;
669 out->m[2][0]=z * x * (1 - c) + y * s;
670 out->m[2][1]=y * z * (1 - c) - x * s;
671 out->m[2][2]=z * z + c * (1 - z * z);
680 void Matrix4x4_CreateScale (matrix4x4_t *out, double x)
700 void Matrix4x4_CreateScale3 (matrix4x4_t *out, double x, double y, double z)
720 void Matrix4x4_CreateFromQuakeEntity(matrix4x4_t *out, double x, double y, double z, double pitch, double yaw, double roll, double scale)
722 double angle, sr, sp, sy, cr, cp, cy;
726 angle = yaw * (M_PI*2 / 360);
729 angle = pitch * (M_PI*2 / 360);
732 angle = roll * (M_PI*2 / 360);
735 #ifdef MATRIX4x4_OPENGLORIENTATION
736 out->m[0][0] = (cp*cy) * scale;
737 out->m[1][0] = (sr*sp*cy+cr*-sy) * scale;
738 out->m[2][0] = (cr*sp*cy+-sr*-sy) * scale;
740 out->m[0][1] = (cp*sy) * scale;
741 out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
742 out->m[2][1] = (cr*sp*sy+-sr*cy) * scale;
744 out->m[0][2] = (-sp) * scale;
745 out->m[1][2] = (sr*cp) * scale;
746 out->m[2][2] = (cr*cp) * scale;
753 out->m[0][0] = (cp*cy) * scale;
754 out->m[0][1] = (sr*sp*cy+cr*-sy) * scale;
755 out->m[0][2] = (cr*sp*cy+-sr*-sy) * scale;
757 out->m[1][0] = (cp*sy) * scale;
758 out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
759 out->m[1][2] = (cr*sp*sy+-sr*cy) * scale;
761 out->m[2][0] = (-sp) * scale;
762 out->m[2][1] = (sr*cp) * scale;
763 out->m[2][2] = (cr*cp) * scale;
773 angle = yaw * (M_PI*2 / 360);
776 angle = pitch * (M_PI*2 / 360);
779 #ifdef MATRIX4x4_OPENGLORIENTATION
780 out->m[0][0] = (cp*cy) * scale;
781 out->m[1][0] = (-sy) * scale;
782 out->m[2][0] = (sp*cy) * scale;
784 out->m[0][1] = (cp*sy) * scale;
785 out->m[1][1] = (cy) * scale;
786 out->m[2][1] = (sp*sy) * scale;
788 out->m[0][2] = (-sp) * scale;
790 out->m[2][2] = (cp) * scale;
797 out->m[0][0] = (cp*cy) * scale;
798 out->m[0][1] = (-sy) * scale;
799 out->m[0][2] = (sp*cy) * scale;
801 out->m[1][0] = (cp*sy) * scale;
802 out->m[1][1] = (cy) * scale;
803 out->m[1][2] = (sp*sy) * scale;
805 out->m[2][0] = (-sp) * scale;
807 out->m[2][2] = (cp) * scale;
817 angle = yaw * (M_PI*2 / 360);
820 #ifdef MATRIX4x4_OPENGLORIENTATION
821 out->m[0][0] = (cy) * scale;
822 out->m[1][0] = (-sy) * scale;
825 out->m[0][1] = (sy) * scale;
826 out->m[1][1] = (cy) * scale;
831 out->m[2][2] = scale;
838 out->m[0][0] = (cy) * scale;
839 out->m[0][1] = (-sy) * scale;
842 out->m[1][0] = (sy) * scale;
843 out->m[1][1] = (cy) * scale;
848 out->m[2][2] = scale;
858 #ifdef MATRIX4x4_OPENGLORIENTATION
859 out->m[0][0] = scale;
864 out->m[1][1] = scale;
869 out->m[2][2] = scale;
876 out->m[0][0] = scale;
881 out->m[1][1] = scale;
886 out->m[2][2] = scale;
896 void Matrix4x4_ToVectors(const matrix4x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
898 #ifdef MATRIX4x4_OPENGLORIENTATION
927 void Matrix4x4_FromVectors(matrix4x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
929 #ifdef MATRIX4x4_OPENGLORIENTATION
930 out->m[0][0] = vx[0];
931 out->m[1][0] = vy[0];
932 out->m[2][0] = vz[0];
934 out->m[0][1] = vx[1];
935 out->m[1][1] = vy[1];
936 out->m[2][1] = vz[1];
938 out->m[0][2] = vx[2];
939 out->m[1][2] = vy[2];
940 out->m[2][2] = vz[2];
947 out->m[0][0] = vx[0];
948 out->m[0][1] = vy[0];
949 out->m[0][2] = vz[0];
951 out->m[1][0] = vx[1];
952 out->m[1][1] = vy[1];
953 out->m[1][2] = vz[1];
955 out->m[2][0] = vx[2];
956 out->m[2][1] = vy[2];
957 out->m[2][2] = vz[2];
966 void Matrix4x4_ToArrayDoubleGL(const matrix4x4_t *in, double out[16])
968 #ifdef MATRIX4x4_OPENGLORIENTATION
969 out[ 0] = in->m[0][0];
970 out[ 1] = in->m[0][1];
971 out[ 2] = in->m[0][2];
972 out[ 3] = in->m[0][3];
973 out[ 4] = in->m[1][0];
974 out[ 5] = in->m[1][1];
975 out[ 6] = in->m[1][2];
976 out[ 7] = in->m[1][3];
977 out[ 8] = in->m[2][0];
978 out[ 9] = in->m[2][1];
979 out[10] = in->m[2][2];
980 out[11] = in->m[2][3];
981 out[12] = in->m[3][0];
982 out[13] = in->m[3][1];
983 out[14] = in->m[3][2];
984 out[15] = in->m[3][3];
986 out[ 0] = in->m[0][0];
987 out[ 1] = in->m[1][0];
988 out[ 2] = in->m[2][0];
989 out[ 3] = in->m[3][0];
990 out[ 4] = in->m[0][1];
991 out[ 5] = in->m[1][1];
992 out[ 6] = in->m[2][1];
993 out[ 7] = in->m[3][1];
994 out[ 8] = in->m[0][2];
995 out[ 9] = in->m[1][2];
996 out[10] = in->m[2][2];
997 out[11] = in->m[3][2];
998 out[12] = in->m[0][3];
999 out[13] = in->m[1][3];
1000 out[14] = in->m[2][3];
1001 out[15] = in->m[3][3];
1005 void Matrix4x4_FromArrayDoubleGL (matrix4x4_t *out, const double in[16])
1007 #ifdef MATRIX4x4_OPENGLORIENTATION
1008 out->m[0][0] = in[0];
1009 out->m[0][1] = in[1];
1010 out->m[0][2] = in[2];
1011 out->m[0][3] = in[3];
1012 out->m[1][0] = in[4];
1013 out->m[1][1] = in[5];
1014 out->m[1][2] = in[6];
1015 out->m[1][3] = in[7];
1016 out->m[2][0] = in[8];
1017 out->m[2][1] = in[9];
1018 out->m[2][2] = in[10];
1019 out->m[2][3] = in[11];
1020 out->m[3][0] = in[12];
1021 out->m[3][1] = in[13];
1022 out->m[3][2] = in[14];
1023 out->m[3][3] = in[15];
1025 out->m[0][0] = in[0];
1026 out->m[1][0] = in[1];
1027 out->m[2][0] = in[2];
1028 out->m[3][0] = in[3];
1029 out->m[0][1] = in[4];
1030 out->m[1][1] = in[5];
1031 out->m[2][1] = in[6];
1032 out->m[3][1] = in[7];
1033 out->m[0][2] = in[8];
1034 out->m[1][2] = in[9];
1035 out->m[2][2] = in[10];
1036 out->m[3][2] = in[11];
1037 out->m[0][3] = in[12];
1038 out->m[1][3] = in[13];
1039 out->m[2][3] = in[14];
1040 out->m[3][3] = in[15];
1044 void Matrix4x4_ToArrayDoubleD3D(const matrix4x4_t *in, double out[16])
1046 #ifdef MATRIX4x4_OPENGLORIENTATION
1047 out[ 0] = in->m[0][0];
1048 out[ 1] = in->m[1][0];
1049 out[ 2] = in->m[2][0];
1050 out[ 3] = in->m[3][0];
1051 out[ 4] = in->m[0][1];
1052 out[ 5] = in->m[1][1];
1053 out[ 6] = in->m[2][1];
1054 out[ 7] = in->m[3][1];
1055 out[ 8] = in->m[0][2];
1056 out[ 9] = in->m[1][2];
1057 out[10] = in->m[2][2];
1058 out[11] = in->m[3][2];
1059 out[12] = in->m[0][3];
1060 out[13] = in->m[1][3];
1061 out[14] = in->m[2][3];
1062 out[15] = in->m[3][3];
1064 out[ 0] = in->m[0][0];
1065 out[ 1] = in->m[0][1];
1066 out[ 2] = in->m[0][2];
1067 out[ 3] = in->m[0][3];
1068 out[ 4] = in->m[1][0];
1069 out[ 5] = in->m[1][1];
1070 out[ 6] = in->m[1][2];
1071 out[ 7] = in->m[1][3];
1072 out[ 8] = in->m[2][0];
1073 out[ 9] = in->m[2][1];
1074 out[10] = in->m[2][2];
1075 out[11] = in->m[2][3];
1076 out[12] = in->m[3][0];
1077 out[13] = in->m[3][1];
1078 out[14] = in->m[3][2];
1079 out[15] = in->m[3][3];
1083 void Matrix4x4_FromArrayDoubleD3D (matrix4x4_t *out, const double in[16])
1085 #ifdef MATRIX4x4_OPENGLORIENTATION
1086 out->m[0][0] = in[0];
1087 out->m[1][0] = in[1];
1088 out->m[2][0] = in[2];
1089 out->m[3][0] = in[3];
1090 out->m[0][1] = in[4];
1091 out->m[1][1] = in[5];
1092 out->m[2][1] = in[6];
1093 out->m[3][1] = in[7];
1094 out->m[0][2] = in[8];
1095 out->m[1][2] = in[9];
1096 out->m[2][2] = in[10];
1097 out->m[3][2] = in[11];
1098 out->m[0][3] = in[12];
1099 out->m[1][3] = in[13];
1100 out->m[2][3] = in[14];
1101 out->m[3][3] = in[15];
1103 out->m[0][0] = in[0];
1104 out->m[0][1] = in[1];
1105 out->m[0][2] = in[2];
1106 out->m[0][3] = in[3];
1107 out->m[1][0] = in[4];
1108 out->m[1][1] = in[5];
1109 out->m[1][2] = in[6];
1110 out->m[1][3] = in[7];
1111 out->m[2][0] = in[8];
1112 out->m[2][1] = in[9];
1113 out->m[2][2] = in[10];
1114 out->m[2][3] = in[11];
1115 out->m[3][0] = in[12];
1116 out->m[3][1] = in[13];
1117 out->m[3][2] = in[14];
1118 out->m[3][3] = in[15];
1122 void Matrix4x4_ToArrayFloatGL(const matrix4x4_t *in, float out[16])
1124 #ifdef MATRIX4x4_OPENGLORIENTATION
1125 out[ 0] = in->m[0][0];
1126 out[ 1] = in->m[0][1];
1127 out[ 2] = in->m[0][2];
1128 out[ 3] = in->m[0][3];
1129 out[ 4] = in->m[1][0];
1130 out[ 5] = in->m[1][1];
1131 out[ 6] = in->m[1][2];
1132 out[ 7] = in->m[1][3];
1133 out[ 8] = in->m[2][0];
1134 out[ 9] = in->m[2][1];
1135 out[10] = in->m[2][2];
1136 out[11] = in->m[2][3];
1137 out[12] = in->m[3][0];
1138 out[13] = in->m[3][1];
1139 out[14] = in->m[3][2];
1140 out[15] = in->m[3][3];
1142 out[ 0] = in->m[0][0];
1143 out[ 1] = in->m[1][0];
1144 out[ 2] = in->m[2][0];
1145 out[ 3] = in->m[3][0];
1146 out[ 4] = in->m[0][1];
1147 out[ 5] = in->m[1][1];
1148 out[ 6] = in->m[2][1];
1149 out[ 7] = in->m[3][1];
1150 out[ 8] = in->m[0][2];
1151 out[ 9] = in->m[1][2];
1152 out[10] = in->m[2][2];
1153 out[11] = in->m[3][2];
1154 out[12] = in->m[0][3];
1155 out[13] = in->m[1][3];
1156 out[14] = in->m[2][3];
1157 out[15] = in->m[3][3];
1161 void Matrix4x4_FromArrayFloatGL (matrix4x4_t *out, const float in[16])
1163 #ifdef MATRIX4x4_OPENGLORIENTATION
1164 out->m[0][0] = in[0];
1165 out->m[0][1] = in[1];
1166 out->m[0][2] = in[2];
1167 out->m[0][3] = in[3];
1168 out->m[1][0] = in[4];
1169 out->m[1][1] = in[5];
1170 out->m[1][2] = in[6];
1171 out->m[1][3] = in[7];
1172 out->m[2][0] = in[8];
1173 out->m[2][1] = in[9];
1174 out->m[2][2] = in[10];
1175 out->m[2][3] = in[11];
1176 out->m[3][0] = in[12];
1177 out->m[3][1] = in[13];
1178 out->m[3][2] = in[14];
1179 out->m[3][3] = in[15];
1181 out->m[0][0] = in[0];
1182 out->m[1][0] = in[1];
1183 out->m[2][0] = in[2];
1184 out->m[3][0] = in[3];
1185 out->m[0][1] = in[4];
1186 out->m[1][1] = in[5];
1187 out->m[2][1] = in[6];
1188 out->m[3][1] = in[7];
1189 out->m[0][2] = in[8];
1190 out->m[1][2] = in[9];
1191 out->m[2][2] = in[10];
1192 out->m[3][2] = in[11];
1193 out->m[0][3] = in[12];
1194 out->m[1][3] = in[13];
1195 out->m[2][3] = in[14];
1196 out->m[3][3] = in[15];
1200 void Matrix4x4_ToArrayFloatD3D(const matrix4x4_t *in, float out[16])
1202 #ifdef MATRIX4x4_OPENGLORIENTATION
1203 out[ 0] = in->m[0][0];
1204 out[ 1] = in->m[1][0];
1205 out[ 2] = in->m[2][0];
1206 out[ 3] = in->m[3][0];
1207 out[ 4] = in->m[0][1];
1208 out[ 5] = in->m[1][1];
1209 out[ 6] = in->m[2][1];
1210 out[ 7] = in->m[3][1];
1211 out[ 8] = in->m[0][2];
1212 out[ 9] = in->m[1][2];
1213 out[10] = in->m[2][2];
1214 out[11] = in->m[3][2];
1215 out[12] = in->m[0][3];
1216 out[13] = in->m[1][3];
1217 out[14] = in->m[2][3];
1218 out[15] = in->m[3][3];
1220 out[ 0] = in->m[0][0];
1221 out[ 1] = in->m[0][1];
1222 out[ 2] = in->m[0][2];
1223 out[ 3] = in->m[0][3];
1224 out[ 4] = in->m[1][0];
1225 out[ 5] = in->m[1][1];
1226 out[ 6] = in->m[1][2];
1227 out[ 7] = in->m[1][3];
1228 out[ 8] = in->m[2][0];
1229 out[ 9] = in->m[2][1];
1230 out[10] = in->m[2][2];
1231 out[11] = in->m[2][3];
1232 out[12] = in->m[3][0];
1233 out[13] = in->m[3][1];
1234 out[14] = in->m[3][2];
1235 out[15] = in->m[3][3];
1239 void Matrix4x4_FromArrayFloatD3D (matrix4x4_t *out, const float in[16])
1241 #ifdef MATRIX4x4_OPENGLORIENTATION
1242 out->m[0][0] = in[0];
1243 out->m[1][0] = in[1];
1244 out->m[2][0] = in[2];
1245 out->m[3][0] = in[3];
1246 out->m[0][1] = in[4];
1247 out->m[1][1] = in[5];
1248 out->m[2][1] = in[6];
1249 out->m[3][1] = in[7];
1250 out->m[0][2] = in[8];
1251 out->m[1][2] = in[9];
1252 out->m[2][2] = in[10];
1253 out->m[3][2] = in[11];
1254 out->m[0][3] = in[12];
1255 out->m[1][3] = in[13];
1256 out->m[2][3] = in[14];
1257 out->m[3][3] = in[15];
1259 out->m[0][0] = in[0];
1260 out->m[0][1] = in[1];
1261 out->m[0][2] = in[2];
1262 out->m[0][3] = in[3];
1263 out->m[1][0] = in[4];
1264 out->m[1][1] = in[5];
1265 out->m[1][2] = in[6];
1266 out->m[1][3] = in[7];
1267 out->m[2][0] = in[8];
1268 out->m[2][1] = in[9];
1269 out->m[2][2] = in[10];
1270 out->m[2][3] = in[11];
1271 out->m[3][0] = in[12];
1272 out->m[3][1] = in[13];
1273 out->m[3][2] = in[14];
1274 out->m[3][3] = in[15];
1278 void Matrix4x4_ToArray12FloatGL(const matrix4x4_t *in, float out[12])
1280 #ifdef MATRIX4x4_OPENGLORIENTATION
1281 out[ 0] = in->m[0][0];
1282 out[ 1] = in->m[0][1];
1283 out[ 2] = in->m[0][2];
1284 out[ 3] = in->m[1][0];
1285 out[ 4] = in->m[1][1];
1286 out[ 5] = in->m[1][2];
1287 out[ 6] = in->m[2][0];
1288 out[ 7] = in->m[2][1];
1289 out[ 8] = in->m[2][2];
1290 out[ 9] = in->m[3][0];
1291 out[10] = in->m[3][1];
1292 out[11] = in->m[3][2];
1294 out[ 0] = in->m[0][0];
1295 out[ 1] = in->m[1][0];
1296 out[ 2] = in->m[2][0];
1297 out[ 3] = in->m[0][1];
1298 out[ 4] = in->m[1][1];
1299 out[ 5] = in->m[2][1];
1300 out[ 6] = in->m[0][2];
1301 out[ 7] = in->m[1][2];
1302 out[ 8] = in->m[2][2];
1303 out[ 9] = in->m[0][3];
1304 out[10] = in->m[1][3];
1305 out[11] = in->m[2][3];
1309 void Matrix4x4_FromArray12FloatGL(matrix4x4_t *out, const float in[12])
1311 #ifdef MATRIX4x4_OPENGLORIENTATION
1312 out->m[0][0] = in[0];
1313 out->m[0][1] = in[1];
1314 out->m[0][2] = in[2];
1316 out->m[1][0] = in[3];
1317 out->m[1][1] = in[4];
1318 out->m[1][2] = in[5];
1320 out->m[2][0] = in[6];
1321 out->m[2][1] = in[7];
1322 out->m[2][2] = in[8];
1324 out->m[3][0] = in[9];
1325 out->m[3][1] = in[10];
1326 out->m[3][2] = in[11];
1329 out->m[0][0] = in[0];
1330 out->m[1][0] = in[1];
1331 out->m[2][0] = in[2];
1333 out->m[0][1] = in[3];
1334 out->m[1][1] = in[4];
1335 out->m[2][1] = in[5];
1337 out->m[0][2] = in[6];
1338 out->m[1][2] = in[7];
1339 out->m[2][2] = in[8];
1341 out->m[0][3] = in[9];
1342 out->m[1][3] = in[10];
1343 out->m[2][3] = in[11];
1348 void Matrix4x4_ToArray12FloatD3D(const matrix4x4_t *in, float out[12])
1350 #ifdef MATRIX4x4_OPENGLORIENTATION
1351 out[ 0] = in->m[0][0];
1352 out[ 1] = in->m[1][0];
1353 out[ 2] = in->m[2][0];
1354 out[ 3] = in->m[3][0];
1355 out[ 4] = in->m[0][1];
1356 out[ 5] = in->m[1][1];
1357 out[ 6] = in->m[2][1];
1358 out[ 7] = in->m[3][1];
1359 out[ 8] = in->m[0][2];
1360 out[ 9] = in->m[1][2];
1361 out[10] = in->m[2][2];
1362 out[11] = in->m[3][2];
1364 out[ 0] = in->m[0][0];
1365 out[ 1] = in->m[0][1];
1366 out[ 2] = in->m[0][2];
1367 out[ 3] = in->m[0][3];
1368 out[ 4] = in->m[1][0];
1369 out[ 5] = in->m[1][1];
1370 out[ 6] = in->m[1][2];
1371 out[ 7] = in->m[1][3];
1372 out[ 8] = in->m[2][0];
1373 out[ 9] = in->m[2][1];
1374 out[10] = in->m[2][2];
1375 out[11] = in->m[2][3];
1379 void Matrix4x4_FromArray12FloatD3D(matrix4x4_t *out, const float in[12])
1381 #ifdef MATRIX4x4_OPENGLORIENTATION
1382 out->m[0][0] = in[0];
1383 out->m[1][0] = in[1];
1384 out->m[2][0] = in[2];
1385 out->m[3][0] = in[3];
1386 out->m[0][1] = in[4];
1387 out->m[1][1] = in[5];
1388 out->m[2][1] = in[6];
1389 out->m[3][1] = in[7];
1390 out->m[0][2] = in[8];
1391 out->m[1][2] = in[9];
1392 out->m[2][2] = in[10];
1393 out->m[3][2] = in[11];
1399 out->m[0][0] = in[0];
1400 out->m[0][1] = in[1];
1401 out->m[0][2] = in[2];
1402 out->m[0][3] = in[3];
1403 out->m[1][0] = in[4];
1404 out->m[1][1] = in[5];
1405 out->m[1][2] = in[6];
1406 out->m[1][3] = in[7];
1407 out->m[2][0] = in[8];
1408 out->m[2][1] = in[9];
1409 out->m[2][2] = in[10];
1410 out->m[2][3] = in[11];
1418 void Matrix4x4_FromOriginQuat(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z, double w)
1420 #ifdef MATRIX4x4_OPENGLORIENTATION
1421 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;
1422 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;
1423 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;
1424 m->m[0][3]= 0 ;m->m[1][3]= 0 ;m->m[2][3]= 0 ;m->m[3][3]=1;
1426 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;
1427 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;
1428 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;
1429 m->m[3][0]= 0 ;m->m[3][1]= 0 ;m->m[3][2]= 0 ;m->m[3][3]=1;
1433 // LordHavoc: I got this code from:
1434 //http://www.doom3world.org/phpbb2/viewtopic.php?t=2884
1435 void Matrix4x4_FromDoom3Joint(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z)
1437 double w = 1.0 - (x*x+y*y+z*z);
1438 w = w > 0.0 ? -sqrt(w) : 0.0;
1439 #ifdef MATRIX4x4_OPENGLORIENTATION
1440 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;
1441 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;
1442 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;
1443 m->m[0][3]= 0 ;m->m[1][3]= 0 ;m->m[2][3]= 0 ;m->m[3][3]=1;
1445 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;
1446 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;
1447 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;
1448 m->m[3][0]= 0 ;m->m[3][1]= 0 ;m->m[3][2]= 0 ;m->m[3][3]=1;
1452 void Matrix4x4_Blend (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2, double blend)
1454 double iblend = 1 - blend;
1455 out->m[0][0] = in1->m[0][0] * iblend + in2->m[0][0] * blend;
1456 out->m[0][1] = in1->m[0][1] * iblend + in2->m[0][1] * blend;
1457 out->m[0][2] = in1->m[0][2] * iblend + in2->m[0][2] * blend;
1458 out->m[0][3] = in1->m[0][3] * iblend + in2->m[0][3] * blend;
1459 out->m[1][0] = in1->m[1][0] * iblend + in2->m[1][0] * blend;
1460 out->m[1][1] = in1->m[1][1] * iblend + in2->m[1][1] * blend;
1461 out->m[1][2] = in1->m[1][2] * iblend + in2->m[1][2] * blend;
1462 out->m[1][3] = in1->m[1][3] * iblend + in2->m[1][3] * blend;
1463 out->m[2][0] = in1->m[2][0] * iblend + in2->m[2][0] * blend;
1464 out->m[2][1] = in1->m[2][1] * iblend + in2->m[2][1] * blend;
1465 out->m[2][2] = in1->m[2][2] * iblend + in2->m[2][2] * blend;
1466 out->m[2][3] = in1->m[2][3] * iblend + in2->m[2][3] * blend;
1467 out->m[3][0] = in1->m[3][0] * iblend + in2->m[3][0] * blend;
1468 out->m[3][1] = in1->m[3][1] * iblend + in2->m[3][1] * blend;
1469 out->m[3][2] = in1->m[3][2] * iblend + in2->m[3][2] * blend;
1470 out->m[3][3] = in1->m[3][3] * iblend + in2->m[3][3] * blend;
1474 void Matrix4x4_Transform (const matrix4x4_t *in, const float v[3], float out[3])
1476 #ifdef MATRIX4x4_OPENGLORIENTATION
1477 out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0] + in->m[3][0];
1478 out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1] + in->m[3][1];
1479 out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2] + in->m[3][2];
1481 out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
1482 out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
1483 out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
1487 void Matrix4x4_Transform4 (const matrix4x4_t *in, const float v[4], float out[4])
1489 #ifdef MATRIX4x4_OPENGLORIENTATION
1490 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];
1491 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];
1492 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];
1493 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];
1495 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];
1496 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];
1497 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];
1498 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];
1502 void Matrix4x4_Transform3x3 (const matrix4x4_t *in, const float v[3], float out[3])
1504 #ifdef MATRIX4x4_OPENGLORIENTATION
1505 out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0];
1506 out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1];
1507 out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2];
1509 out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2];
1510 out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2];
1511 out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2];
1515 void Matrix4x4_TransformPositivePlane(const matrix4x4_t *in, float x, float y, float z, float d, float *o)
1517 #ifdef MATRIX4x4_OPENGLORIENTATION
1518 o[0] = x * in->m[0][0] + y * in->m[1][0] + z * in->m[2][0];
1519 o[1] = x * in->m[0][1] + y * in->m[1][1] + z * in->m[2][1];
1520 o[2] = x * in->m[0][2] + y * in->m[1][2] + z * in->m[2][2];
1521 o[3] = d + (x * in->m[3][0] + y * in->m[3][1] + z * in->m[3][2]);
1523 o[0] = x * in->m[0][0] + y * in->m[0][1] + z * in->m[0][2];
1524 o[1] = x * in->m[1][0] + y * in->m[1][1] + z * in->m[1][2];
1525 o[2] = x * in->m[2][0] + y * in->m[2][1] + z * in->m[2][2];
1526 o[3] = d + (x * in->m[0][3] + y * in->m[1][3] + z * in->m[2][3]);
1530 void Matrix4x4_TransformStandardPlane(const matrix4x4_t *in, float x, float y, float z, float d, float *o)
1532 #ifdef MATRIX4x4_OPENGLORIENTATION
1533 o[0] = x * in->m[0][0] + y * in->m[1][0] + z * in->m[2][0];
1534 o[1] = x * in->m[0][1] + y * in->m[1][1] + z * in->m[2][1];
1535 o[2] = x * in->m[0][2] + y * in->m[1][2] + z * in->m[2][2];
1536 o[3] = d - (x * in->m[3][0] + y * in->m[3][1] + z * in->m[3][2]);
1538 o[0] = x * in->m[0][0] + y * in->m[0][1] + z * in->m[0][2];
1539 o[1] = x * in->m[1][0] + y * in->m[1][1] + z * in->m[1][2];
1540 o[2] = x * in->m[2][0] + y * in->m[2][1] + z * in->m[2][2];
1541 o[3] = d - (x * in->m[0][3] + y * in->m[1][3] + z * in->m[2][3]);
1546 void Matrix4x4_SimpleUntransform (const matrix4x4_t *in, const float v[3], float out[3])
1549 #ifdef MATRIX4x4_OPENGLORIENTATION
1550 t[0] = v[0] - in->m[3][0];
1551 t[1] = v[1] - in->m[3][1];
1552 t[2] = v[2] - in->m[3][2];
1553 out[0] = t[0] * in->m[0][0] + t[1] * in->m[0][1] + t[2] * in->m[0][2];
1554 out[1] = t[0] * in->m[1][0] + t[1] * in->m[1][1] + t[2] * in->m[1][2];
1555 out[2] = t[0] * in->m[2][0] + t[1] * in->m[2][1] + t[2] * in->m[2][2];
1557 t[0] = v[0] - in->m[0][3];
1558 t[1] = v[1] - in->m[1][3];
1559 t[2] = v[2] - in->m[2][3];
1560 out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
1561 out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
1562 out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
1568 void Matrix4x4_ConcatTranslate (matrix4x4_t *out, double x, double y, double z)
1570 matrix4x4_t base, temp;
1572 Matrix4x4_CreateTranslate(&temp, x, y, z);
1573 Matrix4x4_Concat(out, &base, &temp);
1577 void Matrix4x4_ConcatRotate (matrix4x4_t *out, double angle, double x, double y, double z)
1579 matrix4x4_t base, temp;
1581 Matrix4x4_CreateRotate(&temp, angle, x, y, z);
1582 Matrix4x4_Concat(out, &base, &temp);
1586 void Matrix4x4_ConcatScale (matrix4x4_t *out, double x)
1588 matrix4x4_t base, temp;
1590 Matrix4x4_CreateScale(&temp, x);
1591 Matrix4x4_Concat(out, &base, &temp);
1595 void Matrix4x4_ConcatScale3 (matrix4x4_t *out, double x, double y, double z)
1597 matrix4x4_t base, temp;
1599 Matrix4x4_CreateScale3(&temp, x, y, z);
1600 Matrix4x4_Concat(out, &base, &temp);
1603 void Matrix4x4_OriginFromMatrix (const matrix4x4_t *in, float *out)
1605 #ifdef MATRIX4x4_OPENGLORIENTATION
1606 out[0] = in->m[3][0];
1607 out[1] = in->m[3][1];
1608 out[2] = in->m[3][2];
1610 out[0] = in->m[0][3];
1611 out[1] = in->m[1][3];
1612 out[2] = in->m[2][3];
1616 double Matrix4x4_ScaleFromMatrix (const matrix4x4_t *in)
1618 // we only support uniform scaling, so assume the first row is enough
1619 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]);
1622 void Matrix4x4_SetOrigin (matrix4x4_t *out, double x, double y, double z)
1624 #ifdef MATRIX4x4_OPENGLORIENTATION
1635 void Matrix4x4_AdjustOrigin (matrix4x4_t *out, double x, double y, double z)
1637 #ifdef MATRIX4x4_OPENGLORIENTATION
1648 void Matrix4x4_Scale (matrix4x4_t *out, double rotatescale, double originscale)
1650 out->m[0][0] *= rotatescale;
1651 out->m[0][1] *= rotatescale;
1652 out->m[0][2] *= rotatescale;
1653 out->m[1][0] *= rotatescale;
1654 out->m[1][1] *= rotatescale;
1655 out->m[1][2] *= rotatescale;
1656 out->m[2][0] *= rotatescale;
1657 out->m[2][1] *= rotatescale;
1658 out->m[2][2] *= rotatescale;
1659 #ifdef MATRIX4x4_OPENGLORIENTATION
1660 out->m[3][0] *= originscale;
1661 out->m[3][1] *= originscale;
1662 out->m[3][2] *= originscale;
1664 out->m[0][3] *= originscale;
1665 out->m[1][3] *= originscale;
1666 out->m[2][3] *= originscale;