30 extern void deSetQ4M3(deFloat* resq,
const deFloat (*m1)[DE_MATRIX3_COL]);
32 extern void deSetV3M3xyz(deFloat* resv,
const deFloat (*m1)[DE_MATRIX3_COL]);
34 extern void deSetV3M3zyx(deFloat* resv,
const deFloat (*m1)[DE_MATRIX3_COL]);
35 extern void deSetV3M3zyxV3(deFloat* resv,
const deFloat (*m1)[DE_MATRIX3_COL],
const deFloat* last);
37 extern void deLUdecomposeM3M3(deFloat (*lu)[DE_MATRIX3_COL],
const deFloat (*m1)[DE_MATRIX3_COL]);
38 extern void deBackSubstituteV3M3V3(deFloat* x,
const deFloat (*lu)[DE_MATRIX3_COL],
const deFloat* y);
39 extern void deSetM3S2(deFloat (*res)[DE_MATRIX3_COL],
const int axis,
const deFloat angle);
43 DE_MATH_API
void deSetM3S9(deFloat (*res)[DE_MATRIX3_COL],
44 const deFloat a0,
const deFloat a1,
const deFloat a2,
45 const deFloat a3,
const deFloat a4,
const deFloat a5,
46 const deFloat a6,
const deFloat a7,
const deFloat a8)
48 res[0][0] = a0; res[0][1] = a1; res[0][2] = a2;
49 res[1][0] = a3; res[1][1] = a4; res[1][2] = a5;
50 res[2][0] = a6; res[2][1] = a7; res[2][2] = a8;
53 DE_MATH_API
void deZeroM3(deFloat (*res)[DE_MATRIX3_COL])
55 res[0][0] = 0; res[0][1] = 0; res[0][2] = 0;
56 res[1][0] = 0; res[1][1] = 0; res[1][2] = 0;
57 res[2][0] = 0; res[2][1] = 0; res[2][2] = 0;
60 DE_MATH_API
void deIdentityM3(deFloat (*res)[DE_MATRIX3_COL])
62 res[0][0] = 1; res[0][1] = 0; res[0][2] = 0;
63 res[1][0] = 0; res[1][1] = 1; res[1][2] = 0;
64 res[2][0] = 0; res[2][1] = 0; res[2][2] = 1;
67 DE_MATH_API
void deMulM3S1(deFloat (*res)[DE_MATRIX3_COL],
const deFloat s)
69 res[0][0] *= s; res[0][1] *= s; res[0][2] *= s;
70 res[1][0] *= s; res[1][1] *= s; res[1][2] *= s;
71 res[2][0] *= s; res[2][1] *= s; res[2][2] *= s;
74 DE_MATH_API
void deNegateM3M3(deFloat (*res)[DE_MATRIX3_COL],
const deFloat (*m1)[DE_MATRIX3_COL])
76 res[0][0] = -m1[0][0]; res[0][1] = -m1[0][1]; res[0][2] = -m1[0][2];
77 res[1][0] = -m1[1][0]; res[1][1] = -m1[1][1]; res[1][2] = -m1[1][2];
78 res[2][0] = -m1[2][0]; res[2][1] = -m1[2][1]; res[2][2] = -m1[2][2];
81 DE_MATH_API
void deSetM3M3(deFloat (*res)[DE_MATRIX3_COL],
const deFloat (*m1)[DE_MATRIX3_COL])
83 res[0][0] = m1[0][0]; res[0][1] = m1[0][1]; res[0][2] = m1[0][2];
84 res[1][0] = m1[1][0]; res[1][1] = m1[1][1]; res[1][2] = m1[1][2];
85 res[2][0] = m1[2][0]; res[2][1] = m1[2][1]; res[2][2] = m1[2][2];
88 DE_MATH_API
void deAddM3M3(deFloat (*res)[DE_MATRIX3_COL],
const deFloat (*m1)[DE_MATRIX3_COL])
90 res[0][0] += m1[0][0]; res[0][1] += m1[0][1]; res[0][2] += m1[0][2];
91 res[1][0] += m1[1][0]; res[1][1] += m1[1][1]; res[1][2] += m1[1][2];
92 res[2][0] += m1[2][0]; res[2][1] += m1[2][1]; res[2][2] += m1[2][2];
95 DE_MATH_API
void deSubM3M3(deFloat (*res)[DE_MATRIX3_COL],
const deFloat (*m1)[DE_MATRIX3_COL])
97 res[0][0] -= m1[0][0]; res[0][1] -= m1[0][1]; res[0][2] -= m1[0][2];
98 res[1][0] -= m1[1][0]; res[1][1] -= m1[1][1]; res[1][2] -= m1[1][2];
99 res[2][0] -= m1[2][0]; res[2][1] -= m1[2][1]; res[2][2] -= m1[2][2];
103 DE_MATH_API
void deAddM3M3M3(deFloat (*res)[DE_MATRIX3_COL],
const deFloat (*m1)[DE_MATRIX3_COL],
const deFloat (*m2)[DE_MATRIX3_COL])
105 res[0][0] = m1[0][0] + m2[0][0]; res[0][1] = m1[0][1] + m2[0][1]; res[0][2] = m1[0][2] + m2[0][2];
106 res[1][0] = m1[1][0] + m2[1][0]; res[1][1] = m1[1][1] + m2[1][1]; res[1][2] = m1[1][2] + m2[1][2];
107 res[2][0] = m1[2][0] + m2[2][0]; res[2][1] = m1[2][1] + m2[2][1]; res[2][2] = m1[2][2] + m2[2][2];
110 DE_MATH_API
void deSubM3M3M3(deFloat (*res)[DE_MATRIX3_COL],
const deFloat (*m1)[DE_MATRIX3_COL],
const deFloat (*m2)[DE_MATRIX3_COL])
112 res[0][0] = m1[0][0] - m2[0][0]; res[0][1] = m1[0][1] - m2[0][1]; res[0][2] = m1[0][2] - m2[0][2];
113 res[1][0] = m1[1][0] - m2[1][0]; res[1][1] = m1[1][1] - m2[1][1]; res[1][2] = m1[1][2] - m2[1][2];
114 res[2][0] = m1[2][0] - m2[2][0]; res[2][1] = m1[2][1] - m2[2][1]; res[2][2] = m1[2][2] - m2[2][2];
117 DE_MATH_API
void deMulM3M3S1(deFloat (*res)[DE_MATRIX3_COL],
const deFloat (*m1)[DE_MATRIX3_COL],
const deFloat s)
119 res[0][0] = m1[0][0] * s; res[0][1] = m1[0][1] * s; res[0][2] = m1[0][2] * s;
120 res[1][0] = m1[1][0] * s; res[1][1] = m1[1][1] * s; res[1][2] = m1[1][2] * s;
121 res[2][0] = m1[2][0] * s; res[2][1] = m1[2][1] * s; res[2][2] = m1[2][2] * s;
124 DE_MATH_API
void deMulM3M3M3(deFloat (*res)[DE_MATRIX3_COL],
const deFloat (*m1)[DE_MATRIX3_COL],
const deFloat (*m2)[DE_MATRIX3_COL])
126 res[0][0] = m1[0][0] * m2[0][0] + m1[0][1] * m2[1][0] + m1[0][2] * m2[2][0];
127 res[0][1] = m1[0][0] * m2[0][1] + m1[0][1] * m2[1][1] + m1[0][2] * m2[2][1];
128 res[0][2] = m1[0][0] * m2[0][2] + m1[0][1] * m2[1][2] + m1[0][2] * m2[2][2];
129 res[1][0] = m1[1][0] * m2[0][0] + m1[1][1] * m2[1][0] + m1[1][2] * m2[2][0];
130 res[1][1] = m1[1][0] * m2[0][1] + m1[1][1] * m2[1][1] + m1[1][2] * m2[2][1];
131 res[1][2] = m1[1][0] * m2[0][2] + m1[1][1] * m2[1][2] + m1[1][2] * m2[2][2];
132 res[2][0] = m1[2][0] * m2[0][0] + m1[2][1] * m2[1][0] + m1[2][2] * m2[2][0];
133 res[2][1] = m1[2][0] * m2[0][1] + m1[2][1] * m2[1][1] + m1[2][2] * m2[2][1];
134 res[2][2] = m1[2][0] * m2[0][2] + m1[2][1] * m2[1][2] + m1[2][2] * m2[2][2];
138 DE_MATH_API
void deMulM3M3tM3(deFloat (*res)[DE_MATRIX3_COL],
const deFloat (*m1)[DE_MATRIX3_COL],
const deFloat (*m2)[DE_MATRIX3_COL])
140 res[0][0] = m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0];
141 res[0][1] = m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1];
142 res[0][2] = m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2];
143 res[1][0] = m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0];
144 res[1][1] = m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1];
145 res[1][2] = m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2];
146 res[2][0] = m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0];
147 res[2][1] = m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1];
148 res[2][2] = m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2];
152 DE_MATH_API
void deMulM3M3M3t(deFloat (*res)[DE_MATRIX3_COL],
const deFloat (*m1)[DE_MATRIX3_COL],
const deFloat (*m2)[DE_MATRIX3_COL])
154 res[0][0] = m1[0][0] * m2[0][0] + m1[0][1] * m2[0][1] + m1[0][2] * m2[0][2];
155 res[0][1] = m1[0][0] * m2[1][0] + m1[0][1] * m2[1][1] + m1[0][2] * m2[1][2];
156 res[0][2] = m1[0][0] * m2[2][0] + m1[0][1] * m2[2][1] + m1[0][2] * m2[2][2];
157 res[1][0] = m1[1][0] * m2[0][0] + m1[1][1] * m2[0][1] + m1[1][2] * m2[0][2];
158 res[1][1] = m1[1][0] * m2[1][0] + m1[1][1] * m2[1][1] + m1[1][2] * m2[1][2];
159 res[1][2] = m1[1][0] * m2[2][0] + m1[1][1] * m2[2][1] + m1[1][2] * m2[2][2];
160 res[2][0] = m1[2][0] * m2[0][0] + m1[2][1] * m2[0][1] + m1[2][2] * m2[0][2];
161 res[2][1] = m1[2][0] * m2[1][0] + m1[2][1] * m2[1][1] + m1[2][2] * m2[1][2];
162 res[2][2] = m1[2][0] * m2[2][0] + m1[2][1] * m2[2][1] + m1[2][2] * m2[2][2];
165 DE_MATH_API
void deMulV3M3V3(deFloat* resv,
const deFloat (*m1)[DE_MATRIX3_COL],
const deFloat* v2)
167 resv[0] = m1[0][0] * v2[0] + m1[0][1] * v2[1] + m1[0][2] * v2[2];
168 resv[1] = m1[1][0] * v2[0] + m1[1][1] * v2[1] + m1[1][2] * v2[2];
169 resv[2] = m1[2][0] * v2[0] + m1[2][1] * v2[1] + m1[2][2] * v2[2];
173 DE_MATH_API
void deMulV3M3tV3(deFloat* resv,
const deFloat (*m1)[DE_MATRIX3_COL],
const deFloat* v2)
175 resv[0] = m1[0][0] * v2[0] + m1[1][0] * v2[1] + m1[2][0] * v2[2];
176 resv[1] = m1[0][1] * v2[0] + m1[1][1] * v2[1] + m1[2][1] * v2[2];
177 resv[2] = m1[0][2] * v2[0] + m1[1][2] * v2[1] + m1[2][2] * v2[2];
181 DE_MATH_API
void deMulM3M3V3x(deFloat (*res)[DE_MATRIX3_COL],
const deFloat (*m1)[DE_MATRIX3_COL],
const deFloat* v2)
183 res[0][0] = m1[0][1] * v2[2] - m1[0][2] * v2[1];
184 res[0][1] = -m1[0][0] * v2[2] + m1[0][2] * v2[0];
185 res[0][2] = m1[0][0] * v2[1] - m1[0][1] * v2[0];
186 res[1][0] = m1[1][1] * v2[2] - m1[1][2] * v2[1];
187 res[1][1] = -m1[1][0] * v2[2] + m1[1][2] * v2[0];
188 res[1][2] = m1[1][0] * v2[1] - m1[1][1] * v2[0];
189 res[2][0] = m1[2][1] * v2[2] - m1[2][2] * v2[1];
190 res[2][1] = -m1[2][0] * v2[2] + m1[2][2] * v2[0];
191 res[2][2] = m1[2][0] * v2[1] - m1[2][1] * v2[0];
195 DE_MATH_API
void deTransposeM3M3(deFloat (*res)[DE_MATRIX3_COL],
const deFloat (*m1)[DE_MATRIX3_COL])
197 res[0][0] = m1[0][0]; res[0][1] = m1[1][0]; res[0][2] = m1[2][0];
198 res[1][0] = m1[0][1]; res[1][1] = m1[1][1]; res[1][2] = m1[2][1];
199 res[2][0] = m1[0][2]; res[2][1] = m1[1][2]; res[2][2] = m1[2][2];
202 DE_MATH_API
void deColumnV3M3S1(deFloat* resv,
const deFloat (*m1)[DE_MATRIX3_COL],
const int col)
204 resv[0] = m1[0][col];
205 resv[1] = m1[1][col];
206 resv[2] = m1[2][col];
209 DE_MATH_API
void deDiagonalM3V3(deFloat (*res)[DE_MATRIX3_COL],
const deFloat* v1)
211 res[0][0] = v1[0]; res[0][1] = 0; res[0][2] = 0;
212 res[1][0] = 0; res[1][1] = v1[1]; res[1][2] = 0;
213 res[2][0] = 0; res[2][1] = 0; res[2][2] = v1[2];
216 DE_MATH_API
void deDiagonalM3S3(deFloat (*res)[DE_MATRIX3_COL],
const deFloat x,
const deFloat y,
const deFloat z)
218 res[0][0] = x; res[0][1] = 0; res[0][2] = 0;
219 res[1][0] = 0; res[1][1] = y; res[1][2] = 0;
220 res[2][0] = 0; res[2][1] = 0; res[2][2] = z;
223 DE_MATH_API
void deDiagonalV3M3(deFloat* resv,
const deFloat (*m1)[DE_MATRIX3_COL])
230 DE_MATH_API deFloat dedetM3(
const deFloat (*m1)[DE_MATRIX3_COL])
232 return ( m1[0][0] * (m1[1][1] * m1[2][2] - m1[1][2] * m1[2][1])
233 - m1[0][1] * (m1[1][0] * m1[2][2] - m1[1][2] * m1[2][0])
234 + m1[0][2] * (m1[1][0] * m1[2][1] - m1[1][1] * m1[2][0]));
237 DE_MATH_API
void deInvertDetSPDM3M3(deFloat (*res)[DE_MATRIX3_COL],
const deFloat (*m1)[DE_MATRIX3_COL])
239 deFloat ood = 1 / dedetM3(m1);
241 res[0][0] = ood * (m1[1][1] * m1[2][2] - m1[1][2] * m1[2][1]);
242 res[0][1] = -ood * (m1[0][1] * m1[2][2] - m1[0][2] * m1[2][1]);
243 res[0][2] = ood * (m1[0][1] * m1[1][2] - m1[0][2] * m1[1][1]);
244 res[1][0] = res[0][1];
245 res[1][1] = ood * (m1[0][0] * m1[2][2] - m1[0][2] * m1[2][0]);
246 res[1][2] = -ood * (m1[0][0] * m1[1][2] - m1[0][2] * m1[1][0]);
247 res[2][0] = res[0][2];
248 res[2][1] = res[1][2];
249 res[2][2] = ood * (m1[0][0] * m1[1][1] - m1[0][1] * m1[1][0]);
252 DE_MATH_API
void deInvertDetM3M3(deFloat (*res)[DE_MATRIX3_COL],
const deFloat (*m1)[DE_MATRIX3_COL])
254 deFloat ood = 1 / dedetM3(m1);
256 res[0][0] = ood * (m1[1][1] * m1[2][2] - m1[1][2] * m1[2][1]);
257 res[0][1] = -ood * (m1[0][1] * m1[2][2] - m1[0][2] * m1[2][1]);
258 res[0][2] = ood * (m1[0][1] * m1[1][2] - m1[0][2] * m1[1][1]);
259 res[1][0] = -ood * (m1[1][0] * m1[2][2] - m1[1][2] * m1[2][0]);
260 res[1][1] = ood * (m1[0][0] * m1[2][2] - m1[0][2] * m1[2][0]);
261 res[1][2] = -ood * (m1[0][0] * m1[1][2] - m1[0][2] * m1[1][0]);
262 res[2][0] = ood * (m1[1][0] * m1[2][1] - m1[1][1] * m1[2][0]);
263 res[2][1] = -ood * (m1[0][0] * m1[2][1] - m1[0][1] * m1[2][0]);
264 res[2][2] = ood * (m1[0][0] * m1[1][1] - m1[0][1] * m1[1][0]);
268 DE_MATH_API
void deAngularErrorV3M3M3(deFloat* resv,
const deFloat (*m1)[DE_MATRIX3_COL],
const deFloat (*m2)[DE_MATRIX3_COL])
270 resv[0] = -(deFloat)0.5*((m1[1][0] * m2[2][0] + m1[1][1] * m2[2][1] + m1[1][2] * m2[2][2])
271 - (m1[2][0] * m2[1][0] + m1[2][1] * m2[1][1] + m1[2][2] * m2[1][2]));
272 resv[1] = -(deFloat)0.5*((m1[2][0] * m2[0][0] + m1[2][1] * m2[0][1] + m1[2][2] * m2[0][2])
273 - (m1[0][0] * m2[2][0] + m1[0][1] * m2[2][1] + m1[0][2] * m2[2][2]));
274 resv[2] = -(deFloat)0.5*((m1[0][0] * m2[1][0] + m1[0][1] * m2[1][1] + m1[0][2] * m2[1][2])
275 - (m1[1][0] * m2[0][0] + m1[1][1] * m2[0][1] + m1[1][2] * m2[0][2]));
282 DE_MATH_API
void deSetM3Q4(deFloat (*res)[DE_MATRIX3_COL],
const deFloat* q1)
284 res[0][0] = 1 - 2 * (q1[1] * q1[1] + q1[2] * q1[2]);
285 res[0][1] = 2 * (q1[0] * q1[1] - q1[3] * q1[2]);
286 res[0][2] = 2 * (q1[0] * q1[2] + q1[3] * q1[1]);
287 res[1][0] = 2 * (q1[0] * q1[1] + q1[3] * q1[2]);
288 res[1][1] = 1 - 2 * (q1[0] * q1[0] + q1[2] * q1[2]);
289 res[1][2] = 2 * (q1[1] * q1[2] - q1[3] * q1[0]);
290 res[2][0] = 2 * (q1[0] * q1[2] - q1[3] * q1[1]);
291 res[2][1] = 2 * (q1[1] * q1[2] + q1[3] * q1[0]);
292 res[2][2] = 1 - 2 * (q1[0] * q1[0] + q1[1] * q1[1]);
296 DE_MATH_API
void deMulM3V3V3t(deFloat (*res)[DE_MATRIX3_COL],
const deFloat* v1,
const deFloat* v2)
298 res[0][0] = v1[0] * v2[0];
299 res[0][1] = v1[0] * v2[1];
300 res[0][2] = v1[0] * v2[2];
301 res[1][0] = v1[1] * v2[0];
302 res[1][1] = v1[1] * v2[1];
303 res[1][2] = v1[1] * v2[2];
304 res[2][0] = v1[2] * v2[0];
305 res[2][1] = v1[2] * v2[1];
306 res[2][2] = v1[2] * v2[2];
310 DE_MATH_API
void deSetM3V3x(deFloat (*res)[DE_MATRIX3_COL],
const deFloat* v1)
312 res[0][0]= 0; res[0][1]= -v1[2]; res[0][2]= v1[1];
313 res[1][0]= v1[2]; res[1][1]= 0; res[1][2]= -v1[0];
314 res[2][0]= -v1[1]; res[2][1]= v1[0]; res[2][2]= 0;
318 DE_MATH_API
void deMulM3V3xM3(deFloat (*res)[DE_MATRIX3_COL],
const deFloat* v1,
const deFloat (*m2)[DE_MATRIX3_COL])
320 res[0][0]= -v1[2] * m2[1][0] + v1[1] * m2[2][0];
321 res[0][1]= -v1[2] * m2[1][1] + v1[1] * m2[2][1];
322 res[0][2]= -v1[2] * m2[1][2] + v1[1] * m2[2][2];
323 res[1][0]= v1[2] * m2[0][0] - v1[0] * m2[2][0];
324 res[1][1]= v1[2] * m2[0][1] - v1[0] * m2[2][1];
325 res[1][2]= v1[2] * m2[0][2] - v1[0] * m2[2][2];
326 res[2][0]= -v1[1] * m2[0][0] + v1[0] * m2[1][0];
327 res[2][1]= -v1[1] * m2[0][1] + v1[0] * m2[1][1];
328 res[2][2]= -v1[1] * m2[0][2] + v1[0] * m2[1][2];
334 DE_MATH_API
void deSetM3V3S1(deFloat (*res)[DE_MATRIX3_COL],
const deFloat* axis,
const deFloat angle)
338 deFloat c = deCos(angle);
340 deFloat s = deSin(angle);
342 res[0][0] = axis[0] * axis[0] * v + c;
343 res[0][1] = axis[0] * axis[1] * v - axis[2] * s;
344 res[0][2] = axis[0] * axis[2] * v + axis[1] * s;
345 res[1][0] = axis[1] * axis[0] * v + axis[2] * s;
346 res[1][1] = axis[1] * axis[1] * v + c;
347 res[1][2] = axis[1] * axis[2] * v - axis[0] * s;
348 res[2][0] = axis[2] * axis[0] * v - axis[1] * s;
349 res[2][1] = axis[2] * axis[1] * v + axis[0] * s;
350 res[2][2] = axis[2] * axis[2] * v + c;
353 DE_MATH_API
void deSetM3xyzS3(deFloat (*res)[DE_MATRIX3_COL],
const deFloat x,
const deFloat y,
const deFloat z)
355 deFloat cx = deCos(x);
356 deFloat sx = deSin(x);
357 deFloat cy = deCos(y);
358 deFloat sy = deSin(y);
359 deFloat cz = deCos(z);
360 deFloat sz = deSin(z);
363 res[0][1] = -cy * sz;
366 res[1][0] = sx * sy * cz + cx * sz;
367 res[1][1] = -sx * sy * sz + cx * cz;
368 res[1][2] = -sx * cy;
370 res[2][0] = -cx * sy * cz + sx * sz;
371 res[2][1] = cx * sy * sz + sx * cz;
375 DE_MATH_API
void deSetM3zyxS3(deFloat (*res)[DE_MATRIX3_COL],
const deFloat x,
const deFloat y,
const deFloat z)
377 deFloat cx = deCos(x);
378 deFloat sx = deSin(x);
379 deFloat cy = deCos(y);
380 deFloat sy = deSin(y);
381 deFloat cz = deCos(z);
382 deFloat sz = deSin(z);
385 res[1][0] = sx * sy * cz - cx * sz;
386 res[2][0] = cx * sy * cz + sx * sz;
389 res[1][1] = sx * sy * sz + cx * cz;
390 res[2][1] = cx * sy * sz - sx * cz;