include/ode/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 ODE_PURE_INLINE void dZeroVector3(dVector3 res)
  44 {
  45     res[dV3E_X] = REAL(0.0);
  46     res[dV3E_Y] = REAL(0.0);
  47     res[dV3E_Z] = REAL(0.0);
  48 }
  49
  50 ODE_PURE_INLINE void dAssignVector3(dVector3 res, dReal x, dReal y, dReal z)
  51 {
  52     res[dV3E_X] = x;
  53     res[dV3E_Y] = y;
  54     res[dV3E_Z] = z;
  55 }
  56
  57 ODE_PURE_INLINE void dZeroMatrix3(dMatrix3 res)
  58 {
  59     res[dM3E_XX] = REAL(0.0); res[dM3E_XY] = REAL(0.0); res[dM3E_XZ] = REAL(0.0);
  60     res[dM3E_YX] = REAL(0.0); res[dM3E_YY] = REAL(0.0); res[dM3E_YZ] = REAL(0.0);
  61     res[dM3E_ZX] = REAL(0.0); res[dM3E_ZY] = REAL(0.0); res[dM3E_ZZ] = REAL(0.0);
  62 }
  63
  64 ODE_PURE_INLINE void dZeroMatrix4(dMatrix4 res)
  65 {
  66     res[dM4E_XX] = REAL(0.0); res[dM4E_XY] = REAL(0.0); res[dM4E_XZ] = REAL(0.0); res[dM4E_XO] = REAL(0.0);
  67     res[dM4E_YX] = REAL(0.0); res[dM4E_YY] = REAL(0.0); res[dM4E_YZ] = REAL(0.0); res[dM4E_YO] = REAL(0.0);
  68     res[dM4E_ZX] = REAL(0.0); res[dM4E_ZY] = REAL(0.0); res[dM4E_ZZ] = REAL(0.0); res[dM4E_ZO] = REAL(0.0);
  69     res[dM4E_OX] = REAL(0.0); res[dM4E_OY] = REAL(0.0); res[dM4E_OZ] = REAL(0.0); res[dM4E_OO] = REAL(0.0);
  70 }
  71
  72 /* Some vector math */
  73 ODE_PURE_INLINE void dAddVectors3(dReal *res, const dReal *a, const dReal *b)
  74 {
  75   const dReal res_0 = a[0] + b[0];
  76   const dReal res_1 = a[1] + b[1];
  77   const dReal res_2 = a[2] + b[2];
  78   /* Only assign after all the calculations are over to avoid incurring memory aliasing*/
  79   res[0] = res_0; res[1] = res_1; res[2] = res_2;
  80 }
  81
  82 ODE_PURE_INLINE void dSubtractVectors3(dReal *res, const dReal *a, const dReal *b)
  83 {
  84   const dReal res_0 = a[0] - b[0];
  85   const dReal res_1 = a[1] - b[1];
  86   const dReal res_2 = a[2] - b[2];
  87   /* Only assign after all the calculations are over to avoid incurring memory aliasing*/
  88   res[0] = res_0; res[1] = res_1; res[2] = res_2;
  89 }
  90
  91 ODE_PURE_INLINE void dAddVectorScaledVector3(dReal *res, const dReal *a, const dReal *b, dReal b_scale)
  92 {
  93     const dReal res_0 = a[0] + b_scale * b[0];
  94     const dReal res_1 = a[1] + b_scale * b[1];
  95     const dReal res_2 = a[2] + b_scale * b[2];
  96     /* Only assign after all the calculations are over to avoid incurring memory aliasing*/
  97     res[0] = res_0; res[1] = res_1; res[2] = res_2;
  98 }
  99
 100 ODE_PURE_INLINE void dAddScaledVectors3(dReal *res, const dReal *a, const dReal *b, dReal a_scale, dReal b_scale)
 101 {
 102   const dReal res_0 = a_scale * a[0] + b_scale * b[0];
 103   const dReal res_1 = a_scale * a[1] + b_scale * b[1];
 104   const dReal res_2 = a_scale * a[2] + b_scale * b[2];
 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;
 107 }
 108
 109 ODE_PURE_INLINE void dAddThreeScaledVectors3(dReal *res, const dReal *a, const dReal *b, const dReal *c, dReal a_scale, dReal b_scale, dReal c_scale)
 110 {
 111     const dReal res_0 = a_scale * a[0] + b_scale * b[0] + c_scale * c[0];
 112     const dReal res_1 = a_scale * a[1] + b_scale * b[1] + c_scale * c[1];
 113     const dReal res_2 = a_scale * a[2] + b_scale * b[2] + c_scale * c[2];
 114     /* Only assign after all the calculations are over to avoid incurring memory aliasing*/
 115     res[0] = res_0; res[1] = res_1; res[2] = res_2;
 116 }
 117
 118 ODE_PURE_INLINE void dScaleVector3(dReal *res, dReal nScale)
 119 {
 120   res[0] *= nScale ;
 121   res[1] *= nScale ;
 122   res[2] *= nScale ;
 123 }
 124
 125 ODE_PURE_INLINE void dNegateVector3(dReal *res)
 126 {
 127   res[0] = -res[0];
 128   res[1] = -res[1];
 129   res[2] = -res[2];
 130 }
 131
 132 ODE_PURE_INLINE void dCopyVector3(dReal *res, const dReal *a)
 133 {
 134   const dReal res_0 = a[0];
 135   const dReal res_1 = a[1];
 136   const dReal res_2 = a[2];
 137   /* Only assign after all the calculations are over to avoid incurring memory aliasing*/
 138   res[0] = res_0; res[1] = res_1; res[2] = res_2;
 139 }
 140
 141 ODE_PURE_INLINE void dCopyScaledVector3(dReal *res, const dReal *a, dReal nScale)
 142 {
 143   const dReal res_0 = a[0] * nScale;
 144   const dReal res_1 = a[1] * nScale;
 145   const dReal res_2 = a[2] * nScale;
 146   /* Only assign after all the calculations are over to avoid incurring memory aliasing*/
 147   res[0] = res_0; res[1] = res_1; res[2] = res_2;
 148 }
 149
 150 ODE_PURE_INLINE void dCopyNegatedVector3(dReal *res, const dReal *a)
 151 {
 152   const dReal res_0 = -a[0];
 153   const dReal res_1 = -a[1];
 154   const dReal res_2 = -a[2];
 155   /* Only assign after all the calculations are over to avoid incurring memory aliasing*/
 156   res[0] = res_0; res[1] = res_1; res[2] = res_2;
 157 }
 158
 159 ODE_PURE_INLINE void dCopyVector4(dReal *res, const dReal *a)
 160 {
 161   const dReal res_0 = a[0];
 162   const dReal res_1 = a[1];
 163   const dReal res_2 = a[2];
 164   const dReal res_3 = a[3];
 165   /* Only assign after all the calculations are over to avoid incurring memory aliasing*/
 166   res[0] = res_0; res[1] = res_1; res[2] = res_2; res[3] = res_3;
 167 }
 168
 169 ODE_PURE_INLINE void dCopyMatrix4x4(dReal *res, const dReal *a)
 170 {
 171   dCopyVector4(res + 0, a + 0);
 172   dCopyVector4(res + 4, a + 4);
 173   dCopyVector4(res + 8, a + 8);
 174 }
 175
 176 ODE_PURE_INLINE void dCopyMatrix4x3(dReal *res, const dReal *a)
 177 {
 178   dCopyVector3(res + 0, a + 0);
 179   dCopyVector3(res + 4, a + 4);
 180   dCopyVector3(res + 8, a + 8);
 181 }
 182
 183 ODE_PURE_INLINE void dGetMatrixColumn3(dReal *res, const dReal *a, unsigned n)
 184 {
 185   const dReal res_0 = a[n + 0];
 186   const dReal res_1 = a[n + 4];
 187   const dReal res_2 = a[n + 8];
 188   /* Only assign after all the calculations are over to avoid incurring memory aliasing*/
 189   res[0] = res_0; res[1] = res_1; res[2] = res_2;
 190 }
 191
 192 ODE_PURE_INLINE dReal dCalcVectorLength3(const dReal *a)
 193 {
 194   return dSqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
 195 }
 196
 197 ODE_PURE_INLINE dReal dCalcVectorLengthSquare3(const dReal *a)
 198 {
 199   return (a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
 200 }
 201
 202 ODE_PURE_INLINE dReal dCalcPointDepth3(const dReal *test_p, const dReal *plane_p, const dReal *plane_n)
 203 {
 204   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];
 205 }
 206
 207
 208 /*
 209 * 3-way dot product. _dCalcVectorDot3 means that elements of `a' and `b' are spaced
 210 * step_a and step_b indexes apart respectively. dCalcVectorDot3() means dDot311.
 211 */
 212
 213 ODE_PURE_INLINE dReal _dCalcVectorDot3(const dReal *a, const dReal *b, unsigned step_a, unsigned step_b)
 214 {
 215   return a[0] * b[0] + a[step_a] * b[step_b] + a[2 * step_a] * b[2 * step_b];
 216 }
 217
 218
 219 ODE_PURE_INLINE dReal dCalcVectorDot3    (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,1,1); }
 220 ODE_PURE_INLINE dReal dCalcVectorDot3_13 (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,1,3); }
 221 ODE_PURE_INLINE dReal dCalcVectorDot3_31 (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,3,1); }
 222 ODE_PURE_INLINE dReal dCalcVectorDot3_33 (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,3,3); }
 223 ODE_PURE_INLINE dReal dCalcVectorDot3_14 (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,1,4); }
 224 ODE_PURE_INLINE dReal dCalcVectorDot3_41 (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,4,1); }
 225 ODE_PURE_INLINE dReal dCalcVectorDot3_44 (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,4,4); }
 226
 227
 228 /*
 229  * cross product, set res = a x b. _dCalcVectorCross3 means that elements of `res', `a'
 230  * and `b' are spaced step_res, step_a and step_b indexes apart respectively.
 231  * dCalcVectorCross3() means dCross3111.
 232  */
 233
 234 ODE_PURE_INLINE void _dCalcVectorCross3(dReal *res, const dReal *a, const dReal *b, unsigned step_res, unsigned step_a, unsigned step_b)
 235 {
 236   const dReal res_0 = a[  step_a]*b[2*step_b] - a[2*step_a]*b[  step_b];
 237   const dReal res_1 = a[2*step_a]*b[       0] - a[       0]*b[2*step_b];
 238   const dReal res_2 = a[       0]*b[  step_b] - a[  step_a]*b[       0];
 239   /* Only assign after all the calculations are over to avoid incurring memory aliasing*/
 240   res[         0] = res_0;
 241   res[  step_res] = res_1;
 242   res[2*step_res] = res_2;
 243 }
 244
 245 ODE_PURE_INLINE void dCalcVectorCross3    (dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 1, 1, 1); }
 246 ODE_PURE_INLINE void dCalcVectorCross3_114(dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 1, 1, 4); }
 247 ODE_PURE_INLINE void dCalcVectorCross3_141(dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 1, 4, 1); }
 248 ODE_PURE_INLINE void dCalcVectorCross3_144(dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 1, 4, 4); }
 249 ODE_PURE_INLINE void dCalcVectorCross3_411(dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 4, 1, 1); }
 250 ODE_PURE_INLINE void dCalcVectorCross3_414(dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 4, 1, 4); }
 251 ODE_PURE_INLINE void dCalcVectorCross3_441(dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 4, 4, 1); }
 252 ODE_PURE_INLINE void dCalcVectorCross3_444(dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 4, 4, 4); }
 253
 254 ODE_PURE_INLINE void dAddVectorCross3(dReal *res, const dReal *a, const dReal *b)
 255 {
 256   dReal tmp[3];
 257   dCalcVectorCross3(tmp, a, b);
 258   dAddVectors3(res, res, tmp);
 259 }
 260
 261 ODE_PURE_INLINE void dSubtractVectorCross3(dReal *res, const dReal *a, const dReal *b)
 262 {
 263   dReal tmp[3];
 264   dCalcVectorCross3(tmp, a, b);
 265   dSubtractVectors3(res, res, tmp);
 266 }
 267
 268
 269 /*
 270  * set a 3x3 submatrix of A to a matrix such that submatrix(A)*b = a x b.
 271  * A is stored by rows, and has `skip' elements per row. the matrix is
 272  * assumed to be already zero, so this does not write zero elements!
 273  * if (plus,minus) is (+,-) then a positive version will be written.
 274  * if (plus,minus) is (-,+) then a negative version will be written.
 275  */
 276
 277 ODE_PURE_INLINE void dSetCrossMatrixPlus(dReal *res, const dReal *a, unsigned skip)
 278 {
 279   const dReal a_0 = a[0], a_1 = a[1], a_2 = a[2];
 280   res[1] = -a_2;
 281   res[2] = +a_1;
 282   res[skip+0] = +a_2;
 283   res[skip+2] = -a_0;
 284   res[2*skip+0] = -a_1;
 285   res[2*skip+1] = +a_0;
 286 }
 287
 288 ODE_PURE_INLINE void dSetCrossMatrixMinus(dReal *res, const dReal *a, unsigned skip)
 289 {
 290   const dReal a_0 = a[0], a_1 = a[1], a_2 = a[2];
 291   res[1] = +a_2;
 292   res[2] = -a_1;
 293   res[skip+0] = -a_2;
 294   res[skip+2] = +a_0;
 295   res[2*skip+0] = +a_1;
 296   res[2*skip+1] = -a_0;
 297 }
 298
 299
 300 /*
 301  * compute the distance between two 3D-vectors
 302  */
 303
 304 ODE_PURE_INLINE dReal dCalcPointsDistance3(const dReal *a, const dReal *b)
 305 {
 306   dReal res;
 307   dReal tmp[3];
 308   dSubtractVectors3(tmp, a, b);
 309   res = dCalcVectorLength3(tmp);
 310   return res;
 311 }
 312
 313 /*
 314  * special case matrix multiplication, with operator selection
 315  */
 316
 317 ODE_PURE_INLINE void _dMultiplyHelper0_331(dReal *res, const dReal *a, const dReal *b)
 318 {
 319   const dReal res_0 = dCalcVectorDot3(a, b);
 320   const dReal res_1 = dCalcVectorDot3(a + 4, b);
 321   const dReal res_2 = dCalcVectorDot3(a + 8, b);
 322   /* Only assign after all the calculations are over to avoid incurring memory aliasing*/
 323   res[0] = res_0; res[1] = res_1; res[2] = res_2;
 324 }
 325
 326 ODE_PURE_INLINE void _dMultiplyHelper1_331(dReal *res, const dReal *a, const dReal *b)
 327 {
 328   const dReal res_0 = dCalcVectorDot3_41(a, b);
 329   const dReal res_1 = dCalcVectorDot3_41(a + 1, b);
 330   const dReal res_2 = dCalcVectorDot3_41(a + 2, b);
 331   /* Only assign after all the calculations are over to avoid incurring memory aliasing*/
 332   res[0] = res_0; res[1] = res_1; res[2] = res_2;
 333 }
 334
 335 ODE_PURE_INLINE void _dMultiplyHelper0_133(dReal *res, const dReal *a, const dReal *b)
 336 {
 337   _dMultiplyHelper1_331(res, b, a);
 338 }
 339
 340 ODE_PURE_INLINE void _dMultiplyHelper1_133(dReal *res, const dReal *a, const dReal *b)
 341 {
 342   const dReal res_0 = dCalcVectorDot3_44(a, b);
 343   const dReal res_1 = dCalcVectorDot3_44(a + 1, b);
 344   const dReal res_2 = dCalcVectorDot3_44(a + 2, b);
 345   /* Only assign after all the calculations are over to avoid incurring memory aliasing*/
 346   res[0] = res_0; res[1] = res_1; res[2] = res_2;
 347 }
 348
 349 /*
 350 Note: NEVER call any of these functions/macros with the same variable for A and C,
 351 it is not equivalent to A*=B.
 352 */
 353
 354 ODE_PURE_INLINE void dMultiply0_331(dReal *res, const dReal *a, const dReal *b)
 355 {
 356   _dMultiplyHelper0_331(res, a, b);
 357 }
 358
 359 ODE_PURE_INLINE void dMultiply1_331(dReal *res, const dReal *a, const dReal *b)
 360 {
 361   _dMultiplyHelper1_331(res, a, b);
 362 }
 363
 364 ODE_PURE_INLINE void dMultiply0_133(dReal *res, const dReal *a, const dReal *b)
 365 {
 366   _dMultiplyHelper0_133(res, a, b);
 367 }
 368
 369 ODE_PURE_INLINE void dMultiply0_333(dReal *res, const dReal *a, const dReal *b)
 370 {
 371   _dMultiplyHelper0_133(res + 0, a + 0, b);
 372   _dMultiplyHelper0_133(res + 4, a + 4, b);
 373   _dMultiplyHelper0_133(res + 8, a + 8, b);
 374 }
 375
 376 ODE_PURE_INLINE void dMultiply1_333(dReal *res, const dReal *a, const dReal *b)
 377 {
 378   _dMultiplyHelper1_133(res + 0, b, a + 0);
 379   _dMultiplyHelper1_133(res + 4, b, a + 1);
 380   _dMultiplyHelper1_133(res + 8, b, a + 2);
 381 }
 382
 383 ODE_PURE_INLINE void dMultiply2_333(dReal *res, const dReal *a, const dReal *b)
 384 {
 385   _dMultiplyHelper0_331(res + 0, b, a + 0);
 386   _dMultiplyHelper0_331(res + 4, b, a + 4);
 387   _dMultiplyHelper0_331(res + 8, b, a + 8);
 388 }
 389
 390 ODE_PURE_INLINE void dMultiplyAdd0_331(dReal *res, const dReal *a, const dReal *b)
 391 {
 392   dReal tmp[3];
 393   _dMultiplyHelper0_331(tmp, a, b);
 394   dAddVectors3(res, res, tmp);
 395 }
 396
 397 ODE_PURE_INLINE void dMultiplyAdd1_331(dReal *res, const dReal *a, const dReal *b)
 398 {
 399   dReal tmp[3];
 400   _dMultiplyHelper1_331(tmp, a, b);
 401   dAddVectors3(res, res, tmp);
 402 }
 403
 404 ODE_PURE_INLINE void dMultiplyAdd0_133(dReal *res, const dReal *a, const dReal *b)
 405 {
 406   dReal tmp[3];
 407   _dMultiplyHelper0_133(tmp, a, b);
 408   dAddVectors3(res, res, tmp);
 409 }
 410
 411 ODE_PURE_INLINE void dMultiplyAdd0_333(dReal *res, const dReal *a, const dReal *b)
 412 {
 413   dReal tmp[3];
 414   _dMultiplyHelper0_133(tmp, a + 0, b);
 415   dAddVectors3(res+ 0, res + 0, tmp);
 416   _dMultiplyHelper0_133(tmp, a + 4, b);
 417   dAddVectors3(res + 4, res + 4, tmp);
 418   _dMultiplyHelper0_133(tmp, a + 8, b);
 419   dAddVectors3(res + 8, res + 8, tmp);
 420 }
 421
 422 ODE_PURE_INLINE void dMultiplyAdd1_333(dReal *res, const dReal *a, const dReal *b)
 423 {
 424   dReal tmp[3];
 425   _dMultiplyHelper1_133(tmp, b, a + 0);
 426   dAddVectors3(res + 0, res + 0, tmp);
 427   _dMultiplyHelper1_133(tmp, b, a + 1);
 428   dAddVectors3(res + 4, res + 4, tmp);
 429   _dMultiplyHelper1_133(tmp, b, a + 2);
 430   dAddVectors3(res + 8, res + 8, tmp);
 431 }
 432
 433 ODE_PURE_INLINE void dMultiplyAdd2_333(dReal *res, const dReal *a, const dReal *b)
 434 {
 435   dReal tmp[3];
 436   _dMultiplyHelper0_331(tmp, b, a + 0);
 437   dAddVectors3(res + 0, res + 0, tmp);
 438   _dMultiplyHelper0_331(tmp, b, a + 4);
 439   dAddVectors3(res + 4, res + 4, tmp);
 440   _dMultiplyHelper0_331(tmp, b, a + 8);
 441   dAddVectors3(res + 8, res + 8, tmp);
 442 }
 443
 444 ODE_PURE_INLINE dReal dCalcMatrix3Det( const dReal* mat )
 445 {
 446     dReal det;
 447
 448     det = mat[0] * ( mat[5]*mat[10] - mat[9]*mat[6] )
 449         - mat[1] * ( mat[4]*mat[10] - mat[8]*mat[6] )
 450         + mat[2] * ( mat[4]*mat[9]  - mat[8]*mat[5] );
 451
 452     return( det );
 453 }
 454
 455 /**
 456   Closed form matrix inversion, copied from
 457   collision_util.h for use in the stepper.
 458
 459   Returns the determinant.
 460   returns 0 and does nothing
 461   if the matrix is singular.
 462 */
 463 ODE_PURE_INLINE dReal dInvertMatrix3(dReal *dst, const dReal *ma)
 464 {
 465     dReal det;
 466     dReal detRecip;
 467
 468     det = dCalcMatrix3Det( ma );
 469
 470
 471     /* Setting an arbitrary non-zero threshold
 472        for the determinant doesn't do anyone
 473        any favors.  The condition number is the
 474        important thing.  If all the eigen-values
 475        of the matrix are small, so is the
 476        determinant, but it can still be well
 477        conditioned.
 478        A single extremely large eigen-value could
 479        push the determinant over threshold, but
 480        produce a very unstable result if the other
 481        eigen-values are small.  So we just say that
 482        the determinant must be non-zero and trust the
 483        caller to provide well-conditioned matrices.
 484        */
 485     if ( det == 0 )
 486     {
 487         return 0;
 488     }
 489
 490     detRecip = dRecip(det);
 491
 492     dst[0] =  ( ma[5]*ma[10] - ma[6]*ma[9]  ) * detRecip;
 493     dst[1] =  ( ma[9]*ma[2]  - ma[1]*ma[10] ) * detRecip;
 494     dst[2] =  ( ma[1]*ma[6]  - ma[5]*ma[2]  ) * detRecip;
 495
 496     dst[4] =  ( ma[6]*ma[8]  - ma[4]*ma[10] ) * detRecip;
 497     dst[5] =  ( ma[0]*ma[10] - ma[8]*ma[2]  ) * detRecip;
 498     dst[6] =  ( ma[4]*ma[2]  - ma[0]*ma[6]  ) * detRecip;
 499
 500     dst[8] =  ( ma[4]*ma[9]  - ma[8]*ma[5]  ) * detRecip;
 501     dst[9] =  ( ma[8]*ma[1]  - ma[0]*ma[9]  ) * detRecip;
 502     dst[10] = ( ma[0]*ma[5]  - ma[1]*ma[4]  ) * detRecip;
 503
 504     return det;
 505 }
 506
 507
 508 /* Include legacy macros here */
 509 #include <ode/odemath_legacy.h>
 510
 511
 512 #ifdef __cplusplus
 513 extern "C" {
 514 #endif
 515
 516 /*
 517  * normalize 3x1 and 4x1 vectors (i.e. scale them to unit length)
 518  */
 519
 520 /* For DLL export*/
 521 ODE_API int  dSafeNormalize3 (dVector3 a);
 522 ODE_API int  dSafeNormalize4 (dVector4 a);
 523 ODE_API void dNormalize3 (dVector3 a); /* Potentially asserts on zero vec*/
 524 ODE_API void dNormalize4 (dVector4 a); /* Potentially asserts on zero vec*/
 525
 526 /*
 527  * given a unit length "normal" vector n, generate vectors p and q vectors
 528  * that are an orthonormal basis for the plane space perpendicular to n.
 529  * i.e. this makes p,q such that n,p,q are all perpendicular to each other.
 530  * q will equal n x p. if n is not unit length then p will be unit length but
 531  * q wont be.
 532  */
 533
 534 ODE_API void dPlaneSpace (const dVector3 n, dVector3 p, dVector3 q);
 535 /* Makes sure the matrix is a proper rotation, returns a boolean status */
 536 ODE_API int dOrthogonalizeR(dMatrix3 m);
 537
 538
 539
 540 #ifdef __cplusplus
 541 }
 542 #endif
 543
 544
 545 #endif