ODE  0.13.1
 All Data Structures Functions Variables Typedefs Enumerations Groups
odemath.h
1 /*************************************************************************
2  * *
3  * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4  * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5  * *
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 *
12  * file LICENSE.TXT. *
13  * (2) The BSD-style license that is included with this library in *
14  * the file LICENSE-BSD.TXT. *
15  * *
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. *
20  * *
21  *************************************************************************/
22 
23 #ifndef _ODE_ODEMATH_H_
24 #define _ODE_ODEMATH_H_
25 
26 #include <ode/common.h>
27 
28 /*
29  * macro to access elements i,j in an NxM matrix A, independent of the
30  * matrix storage convention.
31  */
32 #define dACCESS33(A,i,j) ((A)[(i)*4+(j)])
33 
34 /*
35  * Macro to test for valid floating point values
36  */
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]) ))
41 
42 
43 
44 /* Some vector math */
45 ODE_PURE_INLINE void dAddVectors3(dReal *res, const dReal *a, const dReal *b)
46 {
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];
50  /* Only assign after all the calculations are over to avoid incurring memory aliasing*/
51  res[0] = res_0; res[1] = res_1; res[2] = res_2;
52 }
53 
54 ODE_PURE_INLINE void dSubtractVectors3(dReal *res, const dReal *a, const dReal *b)
55 {
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];
59  /* Only assign after all the calculations are over to avoid incurring memory aliasing*/
60  res[0] = res_0; res[1] = res_1; res[2] = res_2;
61 }
62 
63 ODE_PURE_INLINE void dAddScaledVectors3(dReal *res, const dReal *a, const dReal *b, dReal a_scale, dReal b_scale)
64 {
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];
68  /* Only assign after all the calculations are over to avoid incurring memory aliasing*/
69  res[0] = res_0; res[1] = res_1; res[2] = res_2;
70 }
71 
72 ODE_PURE_INLINE void dScaleVector3(dReal *res, dReal nScale)
73 {
74  res[0] *= nScale ;
75  res[1] *= nScale ;
76  res[2] *= nScale ;
77 }
78 
79 ODE_PURE_INLINE void dNegateVector3(dReal *res)
80 {
81  res[0] = -res[0];
82  res[1] = -res[1];
83  res[2] = -res[2];
84 }
85 
86 ODE_PURE_INLINE void dCopyVector3(dReal *res, const dReal *a)
87 {
88  const dReal res_0 = a[0];
89  const dReal res_1 = a[1];
90  const dReal res_2 = a[2];
91  /* Only assign after all the calculations are over to avoid incurring memory aliasing*/
92  res[0] = res_0; res[1] = res_1; res[2] = res_2;
93 }
94 
95 ODE_PURE_INLINE void dCopyScaledVector3(dReal *res, const dReal *a, dReal nScale)
96 {
97  const dReal res_0 = a[0] * nScale;
98  const dReal res_1 = a[1] * nScale;
99  const dReal res_2 = a[2] * nScale;
100  /* Only assign after all the calculations are over to avoid incurring memory aliasing*/
101  res[0] = res_0; res[1] = res_1; res[2] = res_2;
102 }
103 
104 ODE_PURE_INLINE void dCopyNegatedVector3(dReal *res, const dReal *a)
105 {
106  const dReal res_0 = -a[0];
107  const dReal res_1 = -a[1];
108  const dReal res_2 = -a[2];
109  /* Only assign after all the calculations are over to avoid incurring memory aliasing*/
110  res[0] = res_0; res[1] = res_1; res[2] = res_2;
111 }
112 
113 ODE_PURE_INLINE void dCopyVector4(dReal *res, const dReal *a)
114 {
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];
119  /* Only assign after all the calculations are over to avoid incurring memory aliasing*/
120  res[0] = res_0; res[1] = res_1; res[2] = res_2; res[3] = res_3;
121 }
122 
123 ODE_PURE_INLINE void dCopyMatrix4x4(dReal *res, const dReal *a)
124 {
125  dCopyVector4(res + 0, a + 0);
126  dCopyVector4(res + 4, a + 4);
127  dCopyVector4(res + 8, a + 8);
128 }
129 
130 ODE_PURE_INLINE void dCopyMatrix4x3(dReal *res, const dReal *a)
131 {
132  dCopyVector3(res + 0, a + 0);
133  dCopyVector3(res + 4, a + 4);
134  dCopyVector3(res + 8, a + 8);
135 }
136 
137 ODE_PURE_INLINE void dGetMatrixColumn3(dReal *res, const dReal *a, unsigned n)
138 {
139  const dReal res_0 = a[n + 0];
140  const dReal res_1 = a[n + 4];
141  const dReal res_2 = a[n + 8];
142  /* Only assign after all the calculations are over to avoid incurring memory aliasing*/
143  res[0] = res_0; res[1] = res_1; res[2] = res_2;
144 }
145 
146 ODE_PURE_INLINE dReal dCalcVectorLength3(const dReal *a)
147 {
148  return dSqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
149 }
150 
151 ODE_PURE_INLINE dReal dCalcVectorLengthSquare3(const dReal *a)
152 {
153  return (a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
154 }
155 
156 ODE_PURE_INLINE dReal dCalcPointDepth3(const dReal *test_p, const dReal *plane_p, const dReal *plane_n)
157 {
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];
159 }
160 
161 
162 /*
163 * 3-way dot product. _dCalcVectorDot3 means that elements of `a' and `b' are spaced
164 * step_a and step_b indexes apart respectively. dCalcVectorDot3() means dDot311.
165 */
166 
167 ODE_PURE_INLINE dReal _dCalcVectorDot3(const dReal *a, const dReal *b, unsigned step_a, unsigned step_b)
168 {
169  return a[0] * b[0] + a[step_a] * b[step_b] + a[2 * step_a] * b[2 * step_b];
170 }
171 
172 
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); }
180 
181 
182 /*
183  * cross product, set res = a x b. _dCalcVectorCross3 means that elements of `res', `a'
184  * and `b' are spaced step_res, step_a and step_b indexes apart respectively.
185  * dCalcVectorCross3() means dCross3111.
186  */
187 
188 ODE_PURE_INLINE void _dCalcVectorCross3(dReal *res, const dReal *a, const dReal *b, unsigned step_res, unsigned step_a, unsigned step_b)
189 {
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];
193  /* Only assign after all the calculations are over to avoid incurring memory aliasing*/
194  res[ 0] = res_0;
195  res[ step_res] = res_1;
196  res[2*step_res] = res_2;
197 }
198 
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); }
207 
208 ODE_PURE_INLINE void dAddVectorCross3(dReal *res, const dReal *a, const dReal *b)
209 {
210  dReal tmp[3];
211  dCalcVectorCross3(tmp, a, b);
212  dAddVectors3(res, res, tmp);
213 }
214 
215 ODE_PURE_INLINE void dSubtractVectorCross3(dReal *res, const dReal *a, const dReal *b)
216 {
217  dReal tmp[3];
218  dCalcVectorCross3(tmp, a, b);
219  dSubtractVectors3(res, res, tmp);
220 }
221 
222 
223 /*
224  * set a 3x3 submatrix of A to a matrix such that submatrix(A)*b = a x b.
225  * A is stored by rows, and has `skip' elements per row. the matrix is
226  * assumed to be already zero, so this does not write zero elements!
227  * if (plus,minus) is (+,-) then a positive version will be written.
228  * if (plus,minus) is (-,+) then a negative version will be written.
229  */
230 
231 ODE_PURE_INLINE void dSetCrossMatrixPlus(dReal *res, const dReal *a, unsigned skip)
232 {
233  const dReal a_0 = a[0], a_1 = a[1], a_2 = a[2];
234  res[1] = -a_2;
235  res[2] = +a_1;
236  res[skip+0] = +a_2;
237  res[skip+2] = -a_0;
238  res[2*skip+0] = -a_1;
239  res[2*skip+1] = +a_0;
240 }
241 
242 ODE_PURE_INLINE void dSetCrossMatrixMinus(dReal *res, const dReal *a, unsigned skip)
243 {
244  const dReal a_0 = a[0], a_1 = a[1], a_2 = a[2];
245  res[1] = +a_2;
246  res[2] = -a_1;
247  res[skip+0] = -a_2;
248  res[skip+2] = +a_0;
249  res[2*skip+0] = +a_1;
250  res[2*skip+1] = -a_0;
251 }
252 
253 
254 /*
255  * compute the distance between two 3D-vectors
256  */
257 
258 ODE_PURE_INLINE dReal dCalcPointsDistance3(const dReal *a, const dReal *b)
259 {
260  dReal res;
261  dReal tmp[3];
262  dSubtractVectors3(tmp, a, b);
263  res = dCalcVectorLength3(tmp);
264  return res;
265 }
266 
267 /*
268  * special case matrix multiplication, with operator selection
269  */
270 
271 ODE_PURE_INLINE void dMultiplyHelper0_331(dReal *res, const dReal *a, const dReal *b)
272 {
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);
276  /* Only assign after all the calculations are over to avoid incurring memory aliasing*/
277  res[0] = res_0; res[1] = res_1; res[2] = res_2;
278 }
279 
280 ODE_PURE_INLINE void dMultiplyHelper1_331(dReal *res, const dReal *a, const dReal *b)
281 {
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);
285  /* Only assign after all the calculations are over to avoid incurring memory aliasing*/
286  res[0] = res_0; res[1] = res_1; res[2] = res_2;
287 }
288 
289 ODE_PURE_INLINE void dMultiplyHelper0_133(dReal *res, const dReal *a, const dReal *b)
290 {
291  dMultiplyHelper1_331(res, b, a);
292 }
293 
294 ODE_PURE_INLINE void dMultiplyHelper1_133(dReal *res, const dReal *a, const dReal *b)
295 {
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);
299  /* Only assign after all the calculations are over to avoid incurring memory aliasing*/
300  res[0] = res_0; res[1] = res_1; res[2] = res_2;
301 }
302 
303 /*
304 Note: NEVER call any of these functions/macros with the same variable for A and C,
305 it is not equivalent to A*=B.
306 */
307 
308 ODE_PURE_INLINE void dMultiply0_331(dReal *res, const dReal *a, const dReal *b)
309 {
310  dMultiplyHelper0_331(res, a, b);
311 }
312 
313 ODE_PURE_INLINE void dMultiply1_331(dReal *res, const dReal *a, const dReal *b)
314 {
315  dMultiplyHelper1_331(res, a, b);
316 }
317 
318 ODE_PURE_INLINE void dMultiply0_133(dReal *res, const dReal *a, const dReal *b)
319 {
320  dMultiplyHelper0_133(res, a, b);
321 }
322 
323 ODE_PURE_INLINE void dMultiply0_333(dReal *res, const dReal *a, const dReal *b)
324 {
325  dMultiplyHelper0_133(res + 0, a + 0, b);
326  dMultiplyHelper0_133(res + 4, a + 4, b);
327  dMultiplyHelper0_133(res + 8, a + 8, b);
328 }
329 
330 ODE_PURE_INLINE void dMultiply1_333(dReal *res, const dReal *a, const dReal *b)
331 {
332  dMultiplyHelper1_133(res + 0, b, a + 0);
333  dMultiplyHelper1_133(res + 4, b, a + 1);
334  dMultiplyHelper1_133(res + 8, b, a + 2);
335 }
336 
337 ODE_PURE_INLINE void dMultiply2_333(dReal *res, const dReal *a, const dReal *b)
338 {
339  dMultiplyHelper0_331(res + 0, b, a + 0);
340  dMultiplyHelper0_331(res + 4, b, a + 4);
341  dMultiplyHelper0_331(res + 8, b, a + 8);
342 }
343 
344 ODE_PURE_INLINE void dMultiplyAdd0_331(dReal *res, const dReal *a, const dReal *b)
345 {
346  dReal tmp[3];
347  dMultiplyHelper0_331(tmp, a, b);
348  dAddVectors3(res, res, tmp);
349 }
350 
351 ODE_PURE_INLINE void dMultiplyAdd1_331(dReal *res, const dReal *a, const dReal *b)
352 {
353  dReal tmp[3];
354  dMultiplyHelper1_331(tmp, a, b);
355  dAddVectors3(res, res, tmp);
356 }
357 
358 ODE_PURE_INLINE void dMultiplyAdd0_133(dReal *res, const dReal *a, const dReal *b)
359 {
360  dReal tmp[3];
361  dMultiplyHelper0_133(tmp, a, b);
362  dAddVectors3(res, res, tmp);
363 }
364 
365 ODE_PURE_INLINE void dMultiplyAdd0_333(dReal *res, const dReal *a, const dReal *b)
366 {
367  dReal tmp[3];
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);
374 }
375 
376 ODE_PURE_INLINE void dMultiplyAdd1_333(dReal *res, const dReal *a, const dReal *b)
377 {
378  dReal tmp[3];
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);
385 }
386 
387 ODE_PURE_INLINE void dMultiplyAdd2_333(dReal *res, const dReal *a, const dReal *b)
388 {
389  dReal tmp[3];
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);
396 }
397 
398 ODE_PURE_INLINE dReal dCalcMatrix3Det( const dReal* mat )
399 {
400  dReal det;
401 
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] );
405 
406  return( det );
407 }
408 
417 ODE_PURE_INLINE dReal dInvertMatrix3(dReal *dst, const dReal *ma)
418 {
419  dReal det;
420  dReal detRecip;
421 
422  det = dCalcMatrix3Det( ma );
423 
424 
425  /* Setting an arbitrary non-zero threshold
426  for the determinant doesn't do anyone
427  any favors. The condition number is the
428  important thing. If all the eigen-values
429  of the matrix are small, so is the
430  determinant, but it can still be well
431  conditioned.
432  A single extremely large eigen-value could
433  push the determinant over threshold, but
434  produce a very unstable result if the other
435  eigen-values are small. So we just say that
436  the determinant must be non-zero and trust the
437  caller to provide well-conditioned matrices.
438  */
439  if ( det == 0 )
440  {
441  return 0;
442  }
443 
444  detRecip = dRecip(det);
445 
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;
449 
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;
453 
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;
457 
458  return det;
459 }
460 
461 
462 /* Include legacy macros here */
463 #include <ode/odemath_legacy.h>
464 
465 
466 #ifdef __cplusplus
467 extern "C" {
468 #endif
469 
470 /*
471  * normalize 3x1 and 4x1 vectors (i.e. scale them to unit length)
472  */
473 
474 /* For DLL export*/
475 ODE_API int dSafeNormalize3 (dVector3 a);
476 ODE_API int dSafeNormalize4 (dVector4 a);
477 ODE_API void dNormalize3 (dVector3 a); /* Potentially asserts on zero vec*/
478 ODE_API void dNormalize4 (dVector4 a); /* Potentially asserts on zero vec*/
479 
480 /*
481  * given a unit length "normal" vector n, generate vectors p and q vectors
482  * that are an orthonormal basis for the plane space perpendicular to n.
483  * i.e. this makes p,q such that n,p,q are all perpendicular to each other.
484  * q will equal n x p. if n is not unit length then p will be unit length but
485  * q wont be.
486  */
487 
488 ODE_API void dPlaneSpace (const dVector3 n, dVector3 p, dVector3 q);
489 /* Makes sure the matrix is a proper rotation */
490 ODE_API void dOrthogonalizeR(dMatrix3 m);
491 
492 
493 
494 #ifdef __cplusplus
495 }
496 #endif
497 
498 
499 #endif