misc/builddeps/linux32/ode/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
  44 // Some vector math
  45 PURE_INLINE void dAddVectors3(dReal *res, const dReal *a, const dReal *b)
  46 {
  47   dReal res_0, res_1, res_2;
  48   res_0 = a[0] + b[0];
  49   res_1 = a[1] + b[1];
  50   res_2 = a[2] + b[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;
  53 }
  54
  55 PURE_INLINE void dSubtractVectors3(dReal *res, const dReal *a, const dReal *b)
  56 {
  57   dReal res_0, res_1, res_2;
  58   res_0 = a[0] - b[0];
  59   res_1 = a[1] - b[1];
  60   res_2 = a[2] - b[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;
  63 }
  64
  65 PURE_INLINE void dAddScaledVectors3(dReal *res, const dReal *a, const dReal *b, dReal a_scale, dReal b_scale)
  66 {
  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;
  73 }
  74
  75 PURE_INLINE void dScaleVector3(dReal *res, dReal nScale)
  76 {
  77   res[0] *= nScale ;
  78   res[1] *= nScale ;
  79   res[2] *= nScale ;
  80 }
  81
  82 PURE_INLINE void dNegateVector3(dReal *res)
  83 {
  84   res[0] = -res[0];
  85   res[1] = -res[1];
  86   res[2] = -res[2];
  87 }
  88
  89 PURE_INLINE void dCopyVector3(dReal *res, const dReal *a)
  90 {
  91   dReal res_0, res_1, res_2;
  92   res_0 = a[0];
  93   res_1 = a[1];
  94   res_2 = a[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;
  97 }
  98
  99 PURE_INLINE void dCopyScaledVector3(dReal *res, const dReal *a, dReal nScale)
 100 {
 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;
 107 }
 108
 109 PURE_INLINE void dCopyNegatedVector3(dReal *res, const dReal *a)
 110 {
 111   dReal res_0, res_1, res_2;
 112   res_0 = -a[0];
 113   res_1 = -a[1];
 114   res_2 = -a[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;
 117 }
 118
 119 PURE_INLINE void dCopyVector4(dReal *res, const dReal *a)
 120 {
 121   dReal res_0, res_1, res_2, res_3;
 122   res_0 = a[0];
 123   res_1 = a[1];
 124   res_2 = a[2];
 125   res_3 = a[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;
 128 }
 129
 130 PURE_INLINE void dCopyMatrix4x4(dReal *res, const dReal *a)
 131 {
 132   dCopyVector4(res + 0, a + 0);
 133   dCopyVector4(res + 4, a + 4);
 134   dCopyVector4(res + 8, a + 8);
 135 }
 136
 137 PURE_INLINE void dCopyMatrix4x3(dReal *res, const dReal *a)
 138 {
 139   dCopyVector3(res + 0, a + 0);
 140   dCopyVector3(res + 4, a + 4);
 141   dCopyVector3(res + 8, a + 8);
 142 }
 143
 144 PURE_INLINE void dGetMatrixColumn3(dReal *res, const dReal *a, unsigned n)
 145 {
 146   dReal res_0, res_1, res_2;
 147   res_0 = a[n + 0];
 148   res_1 = a[n + 4];
 149   res_2 = a[n + 8];
 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;
 152 }
 153
 154 PURE_INLINE dReal dCalcVectorLength3(const dReal *a)
 155 {
 156   return dSqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
 157 }
 158
 159 PURE_INLINE dReal dCalcVectorLengthSquare3(const dReal *a)
 160 {
 161   return (a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
 162 }
 163
 164 PURE_INLINE dReal dCalcPointDepth3(const dReal *test_p, const dReal *plane_p, const dReal *plane_n)
 165 {
 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];
 167 }
 168
 169
 170 /*
 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.
 173 */
 174
 175 PURE_INLINE dReal _dCalcVectorDot3(const dReal *a, const dReal *b, unsigned step_a, unsigned step_b)
 176 {
 177   return a[0] * b[0] + a[step_a] * b[step_b] + a[2 * step_a] * b[2 * step_b];
 178 }
 179
 180
 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); }
 188
 189
 190 /*
 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.
 194  */
 195
 196 PURE_INLINE void _dCalcVectorCross3(dReal *res, const dReal *a, const dReal *b, unsigned step_res, unsigned step_a, unsigned step_b)
 197 {
 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
 203   res[         0] = res_0;
 204   res[  step_res] = res_1;
 205   res[2*step_res] = res_2;
 206 }
 207
 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); }
 216
 217 PURE_INLINE void dAddVectorCross3(dReal *res, const dReal *a, const dReal *b)
 218 {
 219   dReal tmp[3];
 220   dCalcVectorCross3(tmp, a, b);
 221   dAddVectors3(res, res, tmp);
 222 }
 223
 224 PURE_INLINE void dSubtractVectorCross3(dReal *res, const dReal *a, const dReal *b)
 225 {
 226   dReal tmp[3];
 227   dCalcVectorCross3(tmp, a, b);
 228   dSubtractVectors3(res, res, tmp);
 229 }
 230
 231
 232 /*
 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.
 238  */
 239
 240 PURE_INLINE void dSetCrossMatrixPlus(dReal *res, const dReal *a, unsigned skip)
 241 {
 242   const dReal a_0 = a[0], a_1 = a[1], a_2 = a[2];
 243   res[1] = -a_2;
 244   res[2] = +a_1;
 245   res[skip+0] = +a_2;
 246   res[skip+2] = -a_0;
 247   res[2*skip+0] = -a_1;
 248   res[2*skip+1] = +a_0;
 249 }
 250
 251 PURE_INLINE void dSetCrossMatrixMinus(dReal *res, const dReal *a, unsigned skip)
 252 {
 253   const dReal a_0 = a[0], a_1 = a[1], a_2 = a[2];
 254   res[1] = +a_2;
 255   res[2] = -a_1;
 256   res[skip+0] = -a_2;
 257   res[skip+2] = +a_0;
 258   res[2*skip+0] = +a_1;
 259   res[2*skip+1] = -a_0;
 260 }
 261
 262
 263 /*
 264  * compute the distance between two 3D-vectors
 265  */
 266
 267 PURE_INLINE dReal dCalcPointsDistance3(const dReal *a, const dReal *b)
 268 {
 269   dReal res;
 270   dReal tmp[3];
 271   dSubtractVectors3(tmp, a, b);
 272   res = dCalcVectorLength3(tmp);
 273   return res;
 274 }
 275
 276 /*
 277  * special case matrix multiplication, with operator selection
 278  */
 279
 280 PURE_INLINE void dMultiplyHelper0_331(dReal *res, const dReal *a, const dReal *b)
 281 {
 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;
 288 }
 289
 290 PURE_INLINE void dMultiplyHelper1_331(dReal *res, const dReal *a, const dReal *b)
 291 {
 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;
 298 }
 299
 300 PURE_INLINE void dMultiplyHelper0_133(dReal *res, const dReal *a, const dReal *b)
 301 {
 302   dMultiplyHelper1_331(res, b, a);
 303 }
 304
 305 PURE_INLINE void dMultiplyHelper1_133(dReal *res, const dReal *a, const dReal *b)
 306 {
 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;
 313 }
 314
 315 /*
 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.
 318 */
 319
 320 PURE_INLINE void dMultiply0_331(dReal *res, const dReal *a, const dReal *b)
 321 {
 322   dMultiplyHelper0_331(res, a, b);
 323 }
 324
 325 PURE_INLINE void dMultiply1_331(dReal *res, const dReal *a, const dReal *b)
 326 {
 327   dMultiplyHelper1_331(res, a, b);
 328 }
 329
 330 PURE_INLINE void dMultiply0_133(dReal *res, const dReal *a, const dReal *b)
 331 {
 332   dMultiplyHelper0_133(res, a, b);
 333 }
 334
 335 PURE_INLINE void dMultiply0_333(dReal *res, const dReal *a, const dReal *b)
 336 {
 337   dMultiplyHelper0_133(res + 0, a + 0, b);
 338   dMultiplyHelper0_133(res + 4, a + 4, b);
 339   dMultiplyHelper0_133(res + 8, a + 8, b);
 340 }
 341
 342 PURE_INLINE void dMultiply1_333(dReal *res, const dReal *a, const dReal *b)
 343 {
 344   dMultiplyHelper1_133(res + 0, b, a + 0);
 345   dMultiplyHelper1_133(res + 4, b, a + 1);
 346   dMultiplyHelper1_133(res + 8, b, a + 2);
 347 }
 348
 349 PURE_INLINE void dMultiply2_333(dReal *res, const dReal *a, const dReal *b)
 350 {
 351   dMultiplyHelper0_331(res + 0, b, a + 0);
 352   dMultiplyHelper0_331(res + 4, b, a + 4);
 353   dMultiplyHelper0_331(res + 8, b, a + 8);
 354 }
 355
 356 PURE_INLINE void dMultiplyAdd0_331(dReal *res, const dReal *a, const dReal *b)
 357 {
 358   dReal tmp[3];
 359   dMultiplyHelper0_331(tmp, a, b);
 360   dAddVectors3(res, res, tmp);
 361 }
 362
 363 PURE_INLINE void dMultiplyAdd1_331(dReal *res, const dReal *a, const dReal *b)
 364 {
 365   dReal tmp[3];
 366   dMultiplyHelper1_331(tmp, a, b);
 367   dAddVectors3(res, res, tmp);
 368 }
 369
 370 PURE_INLINE void dMultiplyAdd0_133(dReal *res, const dReal *a, const dReal *b)
 371 {
 372   dReal tmp[3];
 373   dMultiplyHelper0_133(tmp, a, b);
 374   dAddVectors3(res, res, tmp);
 375 }
 376
 377 PURE_INLINE void dMultiplyAdd0_333(dReal *res, const dReal *a, const dReal *b)
 378 {
 379   dReal tmp[3];
 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);
 386 }
 387
 388 PURE_INLINE void dMultiplyAdd1_333(dReal *res, const dReal *a, const dReal *b)
 389 {
 390   dReal tmp[3];
 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);
 397 }
 398
 399 PURE_INLINE void dMultiplyAdd2_333(dReal *res, const dReal *a, const dReal *b)
 400 {
 401   dReal tmp[3];
 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);
 408 }
 409
 410
 411 // Include legacy macros here
 412 #include <ode/odemath_legacy.h>
 413
 414
 415 #ifdef __cplusplus
 416 extern "C" {
 417 #endif
 418
 419 /*
 420  * normalize 3x1 and 4x1 vectors (i.e. scale them to unit length)
 421  */
 422
 423 // For DLL export
 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
 428
 429 #if defined(__ODE__)
 430
 431 int  _dSafeNormalize3 (dVector3 a);
 432 int  _dSafeNormalize4 (dVector4 a);
 433
 434 PURE_INLINE void _dNormalize3(dVector3 a)
 435 {
 436   int bNormalizationResult = _dSafeNormalize3(a);
 437   dIVERIFY(bNormalizationResult);
 438 }
 439
 440 PURE_INLINE void _dNormalize4(dVector4 a)
 441 {
 442   int bNormalizationResult = _dSafeNormalize4(a);
 443   dIVERIFY(bNormalizationResult);
 444 }
 445
 446 // For internal use
 447 #define dSafeNormalize3(a) _dSafeNormalize3(a)
 448 #define dSafeNormalize4(a) _dSafeNormalize4(a)
 449 #define dNormalize3(a) _dNormalize3(a)
 450 #define dNormalize4(a) _dNormalize4(a)
 451
 452 #endif // defined(__ODE__)
 453
 454 /*
 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
 459  * q wont be.
 460  */
 461
 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);
 465
 466
 467
 468 #ifdef __cplusplus
 469 }
 470 #endif
 471
 472
 473 #endif