23 #ifndef _ODE_ODEMATH_H_
24 #define _ODE_ODEMATH_H_
26 #include <ode/common.h>
32 #define dACCESS33(A,i,j) ((A)[(i)*4+(j)])
37 #define dVALIDVEC3(v) (!(dIsNan(v[0]) || dIsNan(v[1]) || dIsNan(v[2])))
38 #define dVALIDVEC4(v) (!(dIsNan(v[0]) || dIsNan(v[1]) || dIsNan(v[2]) || dIsNan(v[3])))
39 #define dVALIDMAT3(m) (!(dIsNan(m[0]) || dIsNan(m[1]) || dIsNan(m[2]) || dIsNan(m[3]) || dIsNan(m[4]) || dIsNan(m[5]) || dIsNan(m[6]) || dIsNan(m[7]) || dIsNan(m[8]) || dIsNan(m[9]) || dIsNan(m[10]) || dIsNan(m[11])))
40 #define dVALIDMAT4(m) (!(dIsNan(m[0]) || dIsNan(m[1]) || dIsNan(m[2]) || dIsNan(m[3]) || dIsNan(m[4]) || dIsNan(m[5]) || dIsNan(m[6]) || dIsNan(m[7]) || dIsNan(m[8]) || dIsNan(m[9]) || dIsNan(m[10]) || dIsNan(m[11]) || dIsNan(m[12]) || dIsNan(m[13]) || dIsNan(m[14]) || dIsNan(m[15]) ))
45 ODE_PURE_INLINE
void dAddVectors3(dReal *res,
const dReal *a,
const dReal *b)
47 const dReal res_0 = a[0] + b[0];
48 const dReal res_1 = a[1] + b[1];
49 const dReal res_2 = a[2] + b[2];
51 res[0] = res_0; res[1] = res_1; res[2] = res_2;
54 ODE_PURE_INLINE
void dSubtractVectors3(dReal *res,
const dReal *a,
const dReal *b)
56 const dReal res_0 = a[0] - b[0];
57 const dReal res_1 = a[1] - b[1];
58 const dReal res_2 = a[2] - b[2];
60 res[0] = res_0; res[1] = res_1; res[2] = res_2;
63 ODE_PURE_INLINE
void dAddScaledVectors3(dReal *res,
const dReal *a,
const dReal *b, dReal a_scale, dReal b_scale)
65 const dReal res_0 = a_scale * a[0] + b_scale * b[0];
66 const dReal res_1 = a_scale * a[1] + b_scale * b[1];
67 const dReal res_2 = a_scale * a[2] + b_scale * b[2];
69 res[0] = res_0; res[1] = res_1; res[2] = res_2;
72 ODE_PURE_INLINE
void dScaleVector3(dReal *res, dReal nScale)
79 ODE_PURE_INLINE
void dNegateVector3(dReal *res)
86 ODE_PURE_INLINE
void dCopyVector3(dReal *res,
const dReal *a)
88 const dReal res_0 = a[0];
89 const dReal res_1 = a[1];
90 const dReal res_2 = a[2];
92 res[0] = res_0; res[1] = res_1; res[2] = res_2;
95 ODE_PURE_INLINE
void dCopyScaledVector3(dReal *res,
const dReal *a, dReal nScale)
97 const dReal res_0 = a[0] * nScale;
98 const dReal res_1 = a[1] * nScale;
99 const dReal res_2 = a[2] * nScale;
101 res[0] = res_0; res[1] = res_1; res[2] = res_2;
104 ODE_PURE_INLINE
void dCopyNegatedVector3(dReal *res,
const dReal *a)
106 const dReal res_0 = -a[0];
107 const dReal res_1 = -a[1];
108 const dReal res_2 = -a[2];
110 res[0] = res_0; res[1] = res_1; res[2] = res_2;
113 ODE_PURE_INLINE
void dCopyVector4(dReal *res,
const dReal *a)
115 const dReal res_0 = a[0];
116 const dReal res_1 = a[1];
117 const dReal res_2 = a[2];
118 const dReal res_3 = a[3];
120 res[0] = res_0; res[1] = res_1; res[2] = res_2; res[3] = res_3;
123 ODE_PURE_INLINE
void dCopyMatrix4x4(dReal *res,
const dReal *a)
125 dCopyVector4(res + 0, a + 0);
126 dCopyVector4(res + 4, a + 4);
127 dCopyVector4(res + 8, a + 8);
130 ODE_PURE_INLINE
void dCopyMatrix4x3(dReal *res,
const dReal *a)
132 dCopyVector3(res + 0, a + 0);
133 dCopyVector3(res + 4, a + 4);
134 dCopyVector3(res + 8, a + 8);
137 ODE_PURE_INLINE
void dGetMatrixColumn3(dReal *res,
const dReal *a,
unsigned n)
139 const dReal res_0 = a[n + 0];
140 const dReal res_1 = a[n + 4];
141 const dReal res_2 = a[n + 8];
143 res[0] = res_0; res[1] = res_1; res[2] = res_2;
146 ODE_PURE_INLINE dReal dCalcVectorLength3(
const dReal *a)
148 return dSqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
151 ODE_PURE_INLINE dReal dCalcVectorLengthSquare3(
const dReal *a)
153 return (a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
156 ODE_PURE_INLINE dReal dCalcPointDepth3(
const dReal *test_p,
const dReal *plane_p,
const dReal *plane_n)
158 return (plane_p[0] - test_p[0]) * plane_n[0] + (plane_p[1] - test_p[1]) * plane_n[1] + (plane_p[2] - test_p[2]) * plane_n[2];
167 ODE_PURE_INLINE dReal _dCalcVectorDot3(
const dReal *a,
const dReal *b,
unsigned step_a,
unsigned step_b)
169 return a[0] * b[0] + a[step_a] * b[step_b] + a[2 * step_a] * b[2 * step_b];
173 ODE_PURE_INLINE dReal dCalcVectorDot3 (
const dReal *a,
const dReal *b) {
return _dCalcVectorDot3(a,b,1,1); }
174 ODE_PURE_INLINE dReal dCalcVectorDot3_13 (
const dReal *a,
const dReal *b) {
return _dCalcVectorDot3(a,b,1,3); }
175 ODE_PURE_INLINE dReal dCalcVectorDot3_31 (
const dReal *a,
const dReal *b) {
return _dCalcVectorDot3(a,b,3,1); }
176 ODE_PURE_INLINE dReal dCalcVectorDot3_33 (
const dReal *a,
const dReal *b) {
return _dCalcVectorDot3(a,b,3,3); }
177 ODE_PURE_INLINE dReal dCalcVectorDot3_14 (
const dReal *a,
const dReal *b) {
return _dCalcVectorDot3(a,b,1,4); }
178 ODE_PURE_INLINE dReal dCalcVectorDot3_41 (
const dReal *a,
const dReal *b) {
return _dCalcVectorDot3(a,b,4,1); }
179 ODE_PURE_INLINE dReal dCalcVectorDot3_44 (
const dReal *a,
const dReal *b) {
return _dCalcVectorDot3(a,b,4,4); }
188 ODE_PURE_INLINE
void _dCalcVectorCross3(dReal *res,
const dReal *a,
const dReal *b,
unsigned step_res,
unsigned step_a,
unsigned step_b)
190 const dReal res_0 = a[ step_a]*b[2*step_b] - a[2*step_a]*b[ step_b];
191 const dReal res_1 = a[2*step_a]*b[ 0] - a[ 0]*b[2*step_b];
192 const dReal res_2 = a[ 0]*b[ step_b] - a[ step_a]*b[ 0];
195 res[ step_res] = res_1;
196 res[2*step_res] = res_2;
199 ODE_PURE_INLINE
void dCalcVectorCross3 (dReal *res,
const dReal *a,
const dReal *b) { _dCalcVectorCross3(res, a, b, 1, 1, 1); }
200 ODE_PURE_INLINE
void dCalcVectorCross3_114(dReal *res,
const dReal *a,
const dReal *b) { _dCalcVectorCross3(res, a, b, 1, 1, 4); }
201 ODE_PURE_INLINE
void dCalcVectorCross3_141(dReal *res,
const dReal *a,
const dReal *b) { _dCalcVectorCross3(res, a, b, 1, 4, 1); }
202 ODE_PURE_INLINE
void dCalcVectorCross3_144(dReal *res,
const dReal *a,
const dReal *b) { _dCalcVectorCross3(res, a, b, 1, 4, 4); }
203 ODE_PURE_INLINE
void dCalcVectorCross3_411(dReal *res,
const dReal *a,
const dReal *b) { _dCalcVectorCross3(res, a, b, 4, 1, 1); }
204 ODE_PURE_INLINE
void dCalcVectorCross3_414(dReal *res,
const dReal *a,
const dReal *b) { _dCalcVectorCross3(res, a, b, 4, 1, 4); }
205 ODE_PURE_INLINE
void dCalcVectorCross3_441(dReal *res,
const dReal *a,
const dReal *b) { _dCalcVectorCross3(res, a, b, 4, 4, 1); }
206 ODE_PURE_INLINE
void dCalcVectorCross3_444(dReal *res,
const dReal *a,
const dReal *b) { _dCalcVectorCross3(res, a, b, 4, 4, 4); }
208 ODE_PURE_INLINE
void dAddVectorCross3(dReal *res,
const dReal *a,
const dReal *b)
211 dCalcVectorCross3(tmp, a, b);
212 dAddVectors3(res, res, tmp);
215 ODE_PURE_INLINE
void dSubtractVectorCross3(dReal *res,
const dReal *a,
const dReal *b)
218 dCalcVectorCross3(tmp, a, b);
219 dSubtractVectors3(res, res, tmp);
231 ODE_PURE_INLINE
void dSetCrossMatrixPlus(dReal *res,
const dReal *a,
unsigned skip)
233 const dReal a_0 = a[0], a_1 = a[1], a_2 = a[2];
238 res[2*skip+0] = -a_1;
239 res[2*skip+1] = +a_0;
242 ODE_PURE_INLINE
void dSetCrossMatrixMinus(dReal *res,
const dReal *a,
unsigned skip)
244 const dReal a_0 = a[0], a_1 = a[1], a_2 = a[2];
249 res[2*skip+0] = +a_1;
250 res[2*skip+1] = -a_0;
258 ODE_PURE_INLINE dReal dCalcPointsDistance3(
const dReal *a,
const dReal *b)
262 dSubtractVectors3(tmp, a, b);
263 res = dCalcVectorLength3(tmp);
271 ODE_PURE_INLINE
void dMultiplyHelper0_331(dReal *res,
const dReal *a,
const dReal *b)
273 const dReal res_0 = dCalcVectorDot3(a, b);
274 const dReal res_1 = dCalcVectorDot3(a + 4, b);
275 const dReal res_2 = dCalcVectorDot3(a + 8, b);
277 res[0] = res_0; res[1] = res_1; res[2] = res_2;
280 ODE_PURE_INLINE
void dMultiplyHelper1_331(dReal *res,
const dReal *a,
const dReal *b)
282 const dReal res_0 = dCalcVectorDot3_41(a, b);
283 const dReal res_1 = dCalcVectorDot3_41(a + 1, b);
284 const dReal res_2 = dCalcVectorDot3_41(a + 2, b);
286 res[0] = res_0; res[1] = res_1; res[2] = res_2;
289 ODE_PURE_INLINE
void dMultiplyHelper0_133(dReal *res,
const dReal *a,
const dReal *b)
291 dMultiplyHelper1_331(res, b, a);
294 ODE_PURE_INLINE
void dMultiplyHelper1_133(dReal *res,
const dReal *a,
const dReal *b)
296 const dReal res_0 = dCalcVectorDot3_44(a, b);
297 const dReal res_1 = dCalcVectorDot3_44(a + 1, b);
298 const dReal res_2 = dCalcVectorDot3_44(a + 2, b);
300 res[0] = res_0; res[1] = res_1; res[2] = res_2;
308 ODE_PURE_INLINE
void dMultiply0_331(dReal *res,
const dReal *a,
const dReal *b)
310 dMultiplyHelper0_331(res, a, b);
313 ODE_PURE_INLINE
void dMultiply1_331(dReal *res,
const dReal *a,
const dReal *b)
315 dMultiplyHelper1_331(res, a, b);
318 ODE_PURE_INLINE
void dMultiply0_133(dReal *res,
const dReal *a,
const dReal *b)
320 dMultiplyHelper0_133(res, a, b);
323 ODE_PURE_INLINE
void dMultiply0_333(dReal *res,
const dReal *a,
const dReal *b)
325 dMultiplyHelper0_133(res + 0, a + 0, b);
326 dMultiplyHelper0_133(res + 4, a + 4, b);
327 dMultiplyHelper0_133(res + 8, a + 8, b);
330 ODE_PURE_INLINE
void dMultiply1_333(dReal *res,
const dReal *a,
const dReal *b)
332 dMultiplyHelper1_133(res + 0, b, a + 0);
333 dMultiplyHelper1_133(res + 4, b, a + 1);
334 dMultiplyHelper1_133(res + 8, b, a + 2);
337 ODE_PURE_INLINE
void dMultiply2_333(dReal *res,
const dReal *a,
const dReal *b)
339 dMultiplyHelper0_331(res + 0, b, a + 0);
340 dMultiplyHelper0_331(res + 4, b, a + 4);
341 dMultiplyHelper0_331(res + 8, b, a + 8);
344 ODE_PURE_INLINE
void dMultiplyAdd0_331(dReal *res,
const dReal *a,
const dReal *b)
347 dMultiplyHelper0_331(tmp, a, b);
348 dAddVectors3(res, res, tmp);
351 ODE_PURE_INLINE
void dMultiplyAdd1_331(dReal *res,
const dReal *a,
const dReal *b)
354 dMultiplyHelper1_331(tmp, a, b);
355 dAddVectors3(res, res, tmp);
358 ODE_PURE_INLINE
void dMultiplyAdd0_133(dReal *res,
const dReal *a,
const dReal *b)
361 dMultiplyHelper0_133(tmp, a, b);
362 dAddVectors3(res, res, tmp);
365 ODE_PURE_INLINE
void dMultiplyAdd0_333(dReal *res,
const dReal *a,
const dReal *b)
368 dMultiplyHelper0_133(tmp, a + 0, b);
369 dAddVectors3(res+ 0, res + 0, tmp);
370 dMultiplyHelper0_133(tmp, a + 4, b);
371 dAddVectors3(res + 4, res + 4, tmp);
372 dMultiplyHelper0_133(tmp, a + 8, b);
373 dAddVectors3(res + 8, res + 8, tmp);
376 ODE_PURE_INLINE
void dMultiplyAdd1_333(dReal *res,
const dReal *a,
const dReal *b)
379 dMultiplyHelper1_133(tmp, b, a + 0);
380 dAddVectors3(res + 0, res + 0, tmp);
381 dMultiplyHelper1_133(tmp, b, a + 1);
382 dAddVectors3(res + 4, res + 4, tmp);
383 dMultiplyHelper1_133(tmp, b, a + 2);
384 dAddVectors3(res + 8, res + 8, tmp);
387 ODE_PURE_INLINE
void dMultiplyAdd2_333(dReal *res,
const dReal *a,
const dReal *b)
390 dMultiplyHelper0_331(tmp, b, a + 0);
391 dAddVectors3(res + 0, res + 0, tmp);
392 dMultiplyHelper0_331(tmp, b, a + 4);
393 dAddVectors3(res + 4, res + 4, tmp);
394 dMultiplyHelper0_331(tmp, b, a + 8);
395 dAddVectors3(res + 8, res + 8, tmp);
398 ODE_PURE_INLINE dReal dCalcMatrix3Det(
const dReal* mat )
402 det = mat[0] * ( mat[5]*mat[10] - mat[9]*mat[6] )
403 - mat[1] * ( mat[4]*mat[10] - mat[8]*mat[6] )
404 + mat[2] * ( mat[4]*mat[9] - mat[8]*mat[5] );
417 ODE_PURE_INLINE dReal dInvertMatrix3(dReal *dst,
const dReal *ma)
422 det = dCalcMatrix3Det( ma );
444 detRecip = dRecip(det);
446 dst[0] = ( ma[5]*ma[10] - ma[6]*ma[9] ) * detRecip;
447 dst[1] = ( ma[9]*ma[2] - ma[1]*ma[10] ) * detRecip;
448 dst[2] = ( ma[1]*ma[6] - ma[5]*ma[2] ) * detRecip;
450 dst[4] = ( ma[6]*ma[8] - ma[4]*ma[10] ) * detRecip;
451 dst[5] = ( ma[0]*ma[10] - ma[8]*ma[2] ) * detRecip;
452 dst[6] = ( ma[4]*ma[2] - ma[0]*ma[6] ) * detRecip;
454 dst[8] = ( ma[4]*ma[9] - ma[8]*ma[5] ) * detRecip;
455 dst[9] = ( ma[8]*ma[1] - ma[0]*ma[9] ) * detRecip;
456 dst[10] = ( ma[0]*ma[5] - ma[1]*ma[4] ) * detRecip;
463 #include <ode/odemath_legacy.h>
475 ODE_API
int dSafeNormalize3 (dVector3 a);
476 ODE_API
int dSafeNormalize4 (dVector4 a);
477 ODE_API
void dNormalize3 (dVector3 a);
478 ODE_API
void dNormalize4 (dVector4 a);
488 ODE_API
void dPlaneSpace (
const dVector3 n, dVector3 p, dVector3 q);
490 ODE_API
void dOrthogonalizeR(dMatrix3 m);