1 /*************************************************************************
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
21 *************************************************************************/
23 #ifndef _ODE_ODEMATH_H_
24 #define _ODE_ODEMATH_H_
26 #include <ode/common.h>
29 * macro to access elements i,j in an NxM matrix A, independent of the
30 * matrix storage convention.
32 #define dACCESS33(A,i,j) ((A)[(i)*4+(j)])
35 * Macro to test for valid floating point values
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 PURE_INLINE void dAddVectors3(dReal *res, const dReal *a, const dReal *b)
47 dReal res_0, res_1, res_2;
51 // Only assign after all the calculations are over to avoid incurring memory aliasing
52 res[0] = res_0; res[1] = res_1; res[2] = res_2;
55 PURE_INLINE void dSubtractVectors3(dReal *res, const dReal *a, const dReal *b)
57 dReal res_0, res_1, res_2;
61 // Only assign after all the calculations are over to avoid incurring memory aliasing
62 res[0] = res_0; res[1] = res_1; res[2] = res_2;
65 PURE_INLINE void dAddScaledVectors3(dReal *res, const dReal *a, const dReal *b, dReal a_scale, dReal b_scale)
67 dReal res_0, res_1, res_2;
68 res_0 = a_scale * a[0] + b_scale * b[0];
69 res_1 = a_scale * a[1] + b_scale * b[1];
70 res_2 = a_scale * a[2] + b_scale * b[2];
71 // Only assign after all the calculations are over to avoid incurring memory aliasing
72 res[0] = res_0; res[1] = res_1; res[2] = res_2;
75 PURE_INLINE void dScaleVector3(dReal *res, dReal nScale)
82 PURE_INLINE void dNegateVector3(dReal *res)
89 PURE_INLINE void dCopyVector3(dReal *res, const dReal *a)
91 dReal res_0, res_1, res_2;
95 // Only assign after all the calculations are over to avoid incurring memory aliasing
96 res[0] = res_0; res[1] = res_1; res[2] = res_2;
99 PURE_INLINE void dCopyScaledVector3(dReal *res, const dReal *a, dReal nScale)
101 dReal res_0, res_1, res_2;
102 res_0 = a[0] * nScale;
103 res_1 = a[1] * nScale;
104 res_2 = a[2] * nScale;
105 // Only assign after all the calculations are over to avoid incurring memory aliasing
106 res[0] = res_0; res[1] = res_1; res[2] = res_2;
109 PURE_INLINE void dCopyNegatedVector3(dReal *res, const dReal *a)
111 dReal res_0, res_1, res_2;
115 // Only assign after all the calculations are over to avoid incurring memory aliasing
116 res[0] = res_0; res[1] = res_1; res[2] = res_2;
119 PURE_INLINE void dCopyVector4(dReal *res, const dReal *a)
121 dReal res_0, res_1, res_2, res_3;
126 // Only assign after all the calculations are over to avoid incurring memory aliasing
127 res[0] = res_0; res[1] = res_1; res[2] = res_2; res[3] = res_3;
130 PURE_INLINE void dCopyMatrix4x4(dReal *res, const dReal *a)
132 dCopyVector4(res + 0, a + 0);
133 dCopyVector4(res + 4, a + 4);
134 dCopyVector4(res + 8, a + 8);
137 PURE_INLINE void dCopyMatrix4x3(dReal *res, const dReal *a)
139 dCopyVector3(res + 0, a + 0);
140 dCopyVector3(res + 4, a + 4);
141 dCopyVector3(res + 8, a + 8);
144 PURE_INLINE void dGetMatrixColumn3(dReal *res, const dReal *a, unsigned n)
146 dReal res_0, res_1, res_2;
150 // Only assign after all the calculations are over to avoid incurring memory aliasing
151 res[0] = res_0; res[1] = res_1; res[2] = res_2;
154 PURE_INLINE dReal dCalcVectorLength3(const dReal *a)
156 return dSqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
159 PURE_INLINE dReal dCalcVectorLengthSquare3(const dReal *a)
161 return (a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
164 PURE_INLINE dReal dCalcPointDepth3(const dReal *test_p, const dReal *plane_p, const dReal *plane_n)
166 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];
171 * 3-way dot product. _dCalcVectorDot3 means that elements of `a' and `b' are spaced
172 * step_a and step_b indexes apart respectively. dCalcVectorDot3() means dDot311.
175 PURE_INLINE dReal _dCalcVectorDot3(const dReal *a, const dReal *b, unsigned step_a, unsigned step_b)
177 return a[0] * b[0] + a[step_a] * b[step_b] + a[2 * step_a] * b[2 * step_b];
181 PURE_INLINE dReal dCalcVectorDot3 (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,1,1); }
182 PURE_INLINE dReal dCalcVectorDot3_13 (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,1,3); }
183 PURE_INLINE dReal dCalcVectorDot3_31 (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,3,1); }
184 PURE_INLINE dReal dCalcVectorDot3_33 (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,3,3); }
185 PURE_INLINE dReal dCalcVectorDot3_14 (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,1,4); }
186 PURE_INLINE dReal dCalcVectorDot3_41 (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,4,1); }
187 PURE_INLINE dReal dCalcVectorDot3_44 (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,4,4); }
191 * cross product, set res = a x b. _dCalcVectorCross3 means that elements of `res', `a'
192 * and `b' are spaced step_res, step_a and step_b indexes apart respectively.
193 * dCalcVectorCross3() means dCross3111.
196 PURE_INLINE void _dCalcVectorCross3(dReal *res, const dReal *a, const dReal *b, unsigned step_res, unsigned step_a, unsigned step_b)
198 dReal res_0, res_1, res_2;
199 res_0 = a[ step_a]*b[2*step_b] - a[2*step_a]*b[ step_b];
200 res_1 = a[2*step_a]*b[ 0] - a[ 0]*b[2*step_b];
201 res_2 = a[ 0]*b[ step_b] - a[ step_a]*b[ 0];
202 // Only assign after all the calculations are over to avoid incurring memory aliasing
204 res[ step_res] = res_1;
205 res[2*step_res] = res_2;
208 PURE_INLINE void dCalcVectorCross3 (dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 1, 1, 1); }
209 PURE_INLINE void dCalcVectorCross3_114(dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 1, 1, 4); }
210 PURE_INLINE void dCalcVectorCross3_141(dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 1, 4, 1); }
211 PURE_INLINE void dCalcVectorCross3_144(dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 1, 4, 4); }
212 PURE_INLINE void dCalcVectorCross3_411(dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 4, 1, 1); }
213 PURE_INLINE void dCalcVectorCross3_414(dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 4, 1, 4); }
214 PURE_INLINE void dCalcVectorCross3_441(dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 4, 4, 1); }
215 PURE_INLINE void dCalcVectorCross3_444(dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 4, 4, 4); }
217 PURE_INLINE void dAddVectorCross3(dReal *res, const dReal *a, const dReal *b)
220 dCalcVectorCross3(tmp, a, b);
221 dAddVectors3(res, res, tmp);
224 PURE_INLINE void dSubtractVectorCross3(dReal *res, const dReal *a, const dReal *b)
227 dCalcVectorCross3(tmp, a, b);
228 dSubtractVectors3(res, res, tmp);
233 * set a 3x3 submatrix of A to a matrix such that submatrix(A)*b = a x b.
234 * A is stored by rows, and has `skip' elements per row. the matrix is
235 * assumed to be already zero, so this does not write zero elements!
236 * if (plus,minus) is (+,-) then a positive version will be written.
237 * if (plus,minus) is (-,+) then a negative version will be written.
240 PURE_INLINE void dSetCrossMatrixPlus(dReal *res, const dReal *a, unsigned skip)
242 const dReal a_0 = a[0], a_1 = a[1], a_2 = a[2];
247 res[2*skip+0] = -a_1;
248 res[2*skip+1] = +a_0;
251 PURE_INLINE void dSetCrossMatrixMinus(dReal *res, const dReal *a, unsigned skip)
253 const dReal a_0 = a[0], a_1 = a[1], a_2 = a[2];
258 res[2*skip+0] = +a_1;
259 res[2*skip+1] = -a_0;
264 * compute the distance between two 3D-vectors
267 PURE_INLINE dReal dCalcPointsDistance3(const dReal *a, const dReal *b)
271 dSubtractVectors3(tmp, a, b);
272 res = dCalcVectorLength3(tmp);
277 * special case matrix multiplication, with operator selection
280 PURE_INLINE void dMultiplyHelper0_331(dReal *res, const dReal *a, const dReal *b)
282 dReal res_0, res_1, res_2;
283 res_0 = dCalcVectorDot3(a, b);
284 res_1 = dCalcVectorDot3(a + 4, b);
285 res_2 = dCalcVectorDot3(a + 8, b);
286 // Only assign after all the calculations are over to avoid incurring memory aliasing
287 res[0] = res_0; res[1] = res_1; res[2] = res_2;
290 PURE_INLINE void dMultiplyHelper1_331(dReal *res, const dReal *a, const dReal *b)
292 dReal res_0, res_1, res_2;
293 res_0 = dCalcVectorDot3_41(a, b);
294 res_1 = dCalcVectorDot3_41(a + 1, b);
295 res_2 = dCalcVectorDot3_41(a + 2, b);
296 // Only assign after all the calculations are over to avoid incurring memory aliasing
297 res[0] = res_0; res[1] = res_1; res[2] = res_2;
300 PURE_INLINE void dMultiplyHelper0_133(dReal *res, const dReal *a, const dReal *b)
302 dMultiplyHelper1_331(res, b, a);
305 PURE_INLINE void dMultiplyHelper1_133(dReal *res, const dReal *a, const dReal *b)
307 dReal res_0, res_1, res_2;
308 res_0 = dCalcVectorDot3_44(a, b);
309 res_1 = dCalcVectorDot3_44(a + 1, b);
310 res_2 = dCalcVectorDot3_44(a + 2, b);
311 // Only assign after all the calculations are over to avoid incurring memory aliasing
312 res[0] = res_0; res[1] = res_1; res[2] = res_2;
316 Note: NEVER call any of these functions/macros with the same variable for A and C,
317 it is not equivalent to A*=B.
320 PURE_INLINE void dMultiply0_331(dReal *res, const dReal *a, const dReal *b)
322 dMultiplyHelper0_331(res, a, b);
325 PURE_INLINE void dMultiply1_331(dReal *res, const dReal *a, const dReal *b)
327 dMultiplyHelper1_331(res, a, b);
330 PURE_INLINE void dMultiply0_133(dReal *res, const dReal *a, const dReal *b)
332 dMultiplyHelper0_133(res, a, b);
335 PURE_INLINE void dMultiply0_333(dReal *res, const dReal *a, const dReal *b)
337 dMultiplyHelper0_133(res + 0, a + 0, b);
338 dMultiplyHelper0_133(res + 4, a + 4, b);
339 dMultiplyHelper0_133(res + 8, a + 8, b);
342 PURE_INLINE void dMultiply1_333(dReal *res, const dReal *a, const dReal *b)
344 dMultiplyHelper1_133(res + 0, b, a + 0);
345 dMultiplyHelper1_133(res + 4, b, a + 1);
346 dMultiplyHelper1_133(res + 8, b, a + 2);
349 PURE_INLINE void dMultiply2_333(dReal *res, const dReal *a, const dReal *b)
351 dMultiplyHelper0_331(res + 0, b, a + 0);
352 dMultiplyHelper0_331(res + 4, b, a + 4);
353 dMultiplyHelper0_331(res + 8, b, a + 8);
356 PURE_INLINE void dMultiplyAdd0_331(dReal *res, const dReal *a, const dReal *b)
359 dMultiplyHelper0_331(tmp, a, b);
360 dAddVectors3(res, res, tmp);
363 PURE_INLINE void dMultiplyAdd1_331(dReal *res, const dReal *a, const dReal *b)
366 dMultiplyHelper1_331(tmp, a, b);
367 dAddVectors3(res, res, tmp);
370 PURE_INLINE void dMultiplyAdd0_133(dReal *res, const dReal *a, const dReal *b)
373 dMultiplyHelper0_133(tmp, a, b);
374 dAddVectors3(res, res, tmp);
377 PURE_INLINE void dMultiplyAdd0_333(dReal *res, const dReal *a, const dReal *b)
380 dMultiplyHelper0_133(tmp, a + 0, b);
381 dAddVectors3(res+ 0, res + 0, tmp);
382 dMultiplyHelper0_133(tmp, a + 4, b);
383 dAddVectors3(res + 4, res + 4, tmp);
384 dMultiplyHelper0_133(tmp, a + 8, b);
385 dAddVectors3(res + 8, res + 8, tmp);
388 PURE_INLINE void dMultiplyAdd1_333(dReal *res, const dReal *a, const dReal *b)
391 dMultiplyHelper1_133(tmp, b, a + 0);
392 dAddVectors3(res + 0, res + 0, tmp);
393 dMultiplyHelper1_133(tmp, b, a + 1);
394 dAddVectors3(res + 4, res + 4, tmp);
395 dMultiplyHelper1_133(tmp, b, a + 2);
396 dAddVectors3(res + 8, res + 8, tmp);
399 PURE_INLINE void dMultiplyAdd2_333(dReal *res, const dReal *a, const dReal *b)
402 dMultiplyHelper0_331(tmp, b, a + 0);
403 dAddVectors3(res + 0, res + 0, tmp);
404 dMultiplyHelper0_331(tmp, b, a + 4);
405 dAddVectors3(res + 4, res + 4, tmp);
406 dMultiplyHelper0_331(tmp, b, a + 8);
407 dAddVectors3(res + 8, res + 8, tmp);
411 // Include legacy macros here
412 #include <ode/odemath_legacy.h>
420 * normalize 3x1 and 4x1 vectors (i.e. scale them to unit length)
424 ODE_API int dSafeNormalize3 (dVector3 a);
425 ODE_API int dSafeNormalize4 (dVector4 a);
426 ODE_API void dNormalize3 (dVector3 a); // Potentially asserts on zero vec
427 ODE_API void dNormalize4 (dVector4 a); // Potentially asserts on zero vec
431 int _dSafeNormalize3 (dVector3 a);
432 int _dSafeNormalize4 (dVector4 a);
434 PURE_INLINE void _dNormalize3(dVector3 a)
436 int bNormalizationResult = _dSafeNormalize3(a);
437 dIVERIFY(bNormalizationResult);
440 PURE_INLINE void _dNormalize4(dVector4 a)
442 int bNormalizationResult = _dSafeNormalize4(a);
443 dIVERIFY(bNormalizationResult);
447 #define dSafeNormalize3(a) _dSafeNormalize3(a)
448 #define dSafeNormalize4(a) _dSafeNormalize4(a)
449 #define dNormalize3(a) _dNormalize3(a)
450 #define dNormalize4(a) _dNormalize4(a)
452 #endif // defined(__ODE__)
455 * given a unit length "normal" vector n, generate vectors p and q vectors
456 * that are an orthonormal basis for the plane space perpendicular to n.
457 * i.e. this makes p,q such that n,p,q are all perpendicular to each other.
458 * q will equal n x p. if n is not unit length then p will be unit length but
462 ODE_API void dPlaneSpace (const dVector3 n, dVector3 p, dVector3 q);
463 /* Makes sure the matrix is a proper rotation */
464 ODE_API void dOrthogonalizeR(dMatrix3 m);