Open Dynamics Engine

odemath.h

00001 /*************************************************************************
00002  *                                                                       *
00003  * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
00004  * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
00005  *                                                                       *
00006  * This library is free software; you can redistribute it and/or         *
00007  * modify it under the terms of EITHER:                                  *
00008  *   (1) The GNU Lesser General Public License as published by the Free  *
00009  *       Software Foundation; either version 2.1 of the License, or (at  *
00010  *       your option) any later version. The text of the GNU Lesser      *
00011  *       General Public License is included with this library in the     *
00012  *       file LICENSE.TXT.                                               *
00013  *   (2) The BSD-style license that is included with this library in     *
00014  *       the file LICENSE-BSD.TXT.                                       *
00015  *                                                                       *
00016  * This library is distributed in the hope that it will be useful,       *
00017  * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00018  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
00019  * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
00020  *                                                                       *
00021  *************************************************************************/
00022 
00023 #ifndef _ODE_ODEMATH_H_
00024 #define _ODE_ODEMATH_H_
00025 
00026 #include <ode/common.h>
00027 
00028 /*
00029  * macro to access elements i,j in an NxM matrix A, independent of the
00030  * matrix storage convention.
00031  */
00032 #define dACCESS33(A,i,j) ((A)[(i)*4+(j)])
00033 
00034 /*
00035  * Macro to test for valid floating point values
00036  */
00037 #define dVALIDVEC3(v) (!(dIsNan(v[0]) || dIsNan(v[1]) || dIsNan(v[2])))
00038 #define dVALIDVEC4(v) (!(dIsNan(v[0]) || dIsNan(v[1]) || dIsNan(v[2]) || dIsNan(v[3])))
00039 #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])))
00040 #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]) ))
00041 
00042 
00043 
00044 // Some vector math
00045 PURE_INLINE void dAddVectors3(dReal *res, const dReal *a, const dReal *b)
00046 {
00047   dReal res_0, res_1, res_2;
00048   res_0 = a[0] + b[0];
00049   res_1 = a[1] + b[1];
00050   res_2 = a[2] + b[2];
00051   // Only assign after all the calculations are over to avoid incurring memory aliasing
00052   res[0] = res_0; res[1] = res_1; res[2] = res_2;
00053 }
00054 
00055 PURE_INLINE void dSubtractVectors3(dReal *res, const dReal *a, const dReal *b)
00056 {
00057   dReal res_0, res_1, res_2;
00058   res_0 = a[0] - b[0];
00059   res_1 = a[1] - b[1];
00060   res_2 = a[2] - b[2];
00061   // Only assign after all the calculations are over to avoid incurring memory aliasing
00062   res[0] = res_0; res[1] = res_1; res[2] = res_2;
00063 }
00064 
00065 PURE_INLINE void dAddScaledVectors3(dReal *res, const dReal *a, const dReal *b, dReal a_scale, dReal b_scale)
00066 {
00067   dReal res_0, res_1, res_2;
00068   res_0 = a_scale * a[0] + b_scale * b[0];
00069   res_1 = a_scale * a[1] + b_scale * b[1];
00070   res_2 = a_scale * a[2] + b_scale * b[2];
00071   // Only assign after all the calculations are over to avoid incurring memory aliasing
00072   res[0] = res_0; res[1] = res_1; res[2] = res_2;
00073 }
00074 
00075 PURE_INLINE void dScaleVector3(dReal *res, dReal nScale)
00076 {
00077   res[0] *= nScale ;
00078   res[1] *= nScale ;
00079   res[2] *= nScale ;
00080 }
00081 
00082 PURE_INLINE void dNegateVector3(dReal *res)
00083 {
00084   res[0] = -res[0];
00085   res[1] = -res[1];
00086   res[2] = -res[2];
00087 }
00088 
00089 PURE_INLINE void dCopyVector3(dReal *res, const dReal *a)
00090 {
00091   dReal res_0, res_1, res_2;
00092   res_0 = a[0];
00093   res_1 = a[1];
00094   res_2 = a[2];
00095   // Only assign after all the calculations are over to avoid incurring memory aliasing
00096   res[0] = res_0; res[1] = res_1; res[2] = res_2;
00097 }
00098 
00099 PURE_INLINE void dCopyScaledVector3(dReal *res, const dReal *a, dReal nScale)
00100 {
00101   dReal res_0, res_1, res_2;
00102   res_0 = a[0] * nScale;
00103   res_1 = a[1] * nScale;
00104   res_2 = a[2] * nScale;
00105   // Only assign after all the calculations are over to avoid incurring memory aliasing
00106   res[0] = res_0; res[1] = res_1; res[2] = res_2;
00107 }
00108 
00109 PURE_INLINE void dCopyNegatedVector3(dReal *res, const dReal *a)
00110 {
00111   dReal res_0, res_1, res_2;
00112   res_0 = -a[0];
00113   res_1 = -a[1];
00114   res_2 = -a[2];
00115   // Only assign after all the calculations are over to avoid incurring memory aliasing
00116   res[0] = res_0; res[1] = res_1; res[2] = res_2;
00117 }
00118 
00119 PURE_INLINE void dCopyVector4(dReal *res, const dReal *a)
00120 {
00121   dReal res_0, res_1, res_2, res_3;
00122   res_0 = a[0];
00123   res_1 = a[1];
00124   res_2 = a[2];
00125   res_3 = a[3];
00126   // Only assign after all the calculations are over to avoid incurring memory aliasing
00127   res[0] = res_0; res[1] = res_1; res[2] = res_2; res[3] = res_3;
00128 }
00129 
00130 PURE_INLINE void dCopyMatrix4x4(dReal *res, const dReal *a)
00131 {
00132   dCopyVector4(res + 0, a + 0);
00133   dCopyVector4(res + 4, a + 4);
00134   dCopyVector4(res + 8, a + 8);
00135 }
00136 
00137 PURE_INLINE void dCopyMatrix4x3(dReal *res, const dReal *a)
00138 {
00139   dCopyVector3(res + 0, a + 0);
00140   dCopyVector3(res + 4, a + 4);
00141   dCopyVector3(res + 8, a + 8);
00142 }
00143 
00144 PURE_INLINE void dGetMatrixColumn3(dReal *res, const dReal *a, unsigned n)
00145 {
00146   dReal res_0, res_1, res_2;
00147   res_0 = a[n + 0];
00148   res_1 = a[n + 4];
00149   res_2 = a[n + 8];
00150   // Only assign after all the calculations are over to avoid incurring memory aliasing
00151   res[0] = res_0; res[1] = res_1; res[2] = res_2;
00152 }
00153 
00154 PURE_INLINE dReal dCalcVectorLength3(const dReal *a)
00155 {
00156   return dSqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
00157 }
00158 
00159 PURE_INLINE dReal dCalcVectorLengthSquare3(const dReal *a)
00160 {
00161   return (a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
00162 }
00163 
00164 PURE_INLINE dReal dCalcPointDepth3(const dReal *test_p, const dReal *plane_p, const dReal *plane_n)
00165 {
00166   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];
00167 }
00168 
00169 
00170 /*
00171 * 3-way dot product. _dCalcVectorDot3 means that elements of `a' and `b' are spaced
00172 * step_a and step_b indexes apart respectively. dCalcVectorDot3() means dDot311.
00173 */
00174 
00175 PURE_INLINE dReal _dCalcVectorDot3(const dReal *a, const dReal *b, unsigned step_a, unsigned step_b)
00176 {
00177   return a[0] * b[0] + a[step_a] * b[step_b] + a[2 * step_a] * b[2 * step_b];
00178 }
00179 
00180 
00181 PURE_INLINE dReal dCalcVectorDot3    (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,1,1); }
00182 PURE_INLINE dReal dCalcVectorDot3_13 (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,1,3); }
00183 PURE_INLINE dReal dCalcVectorDot3_31 (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,3,1); }
00184 PURE_INLINE dReal dCalcVectorDot3_33 (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,3,3); }
00185 PURE_INLINE dReal dCalcVectorDot3_14 (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,1,4); }
00186 PURE_INLINE dReal dCalcVectorDot3_41 (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,4,1); }
00187 PURE_INLINE dReal dCalcVectorDot3_44 (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,4,4); }
00188 
00189 
00190 /*
00191  * cross product, set res = a x b. _dCalcVectorCross3 means that elements of `res', `a'
00192  * and `b' are spaced step_res, step_a and step_b indexes apart respectively.
00193  * dCalcVectorCross3() means dCross3111. 
00194  */
00195 
00196 PURE_INLINE void _dCalcVectorCross3(dReal *res, const dReal *a, const dReal *b, unsigned step_res, unsigned step_a, unsigned step_b)
00197 {
00198   dReal res_0, res_1, res_2;
00199   res_0 = a[  step_a]*b[2*step_b] - a[2*step_a]*b[  step_b];
00200   res_1 = a[2*step_a]*b[       0] - a[       0]*b[2*step_b];
00201   res_2 = a[       0]*b[  step_b] - a[  step_a]*b[       0];
00202   // Only assign after all the calculations are over to avoid incurring memory aliasing
00203   res[         0] = res_0;
00204   res[  step_res] = res_1;
00205   res[2*step_res] = res_2;
00206 }
00207 
00208 PURE_INLINE void dCalcVectorCross3    (dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 1, 1, 1); }
00209 PURE_INLINE void dCalcVectorCross3_114(dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 1, 1, 4); }
00210 PURE_INLINE void dCalcVectorCross3_141(dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 1, 4, 1); }
00211 PURE_INLINE void dCalcVectorCross3_144(dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 1, 4, 4); }
00212 PURE_INLINE void dCalcVectorCross3_411(dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 4, 1, 1); }
00213 PURE_INLINE void dCalcVectorCross3_414(dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 4, 1, 4); }
00214 PURE_INLINE void dCalcVectorCross3_441(dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 4, 4, 1); }
00215 PURE_INLINE void dCalcVectorCross3_444(dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 4, 4, 4); }
00216 
00217 PURE_INLINE void dAddVectorCross3(dReal *res, const dReal *a, const dReal *b)
00218 {
00219   dReal tmp[3];
00220   dCalcVectorCross3(tmp, a, b);
00221   dAddVectors3(res, res, tmp);
00222 }
00223 
00224 PURE_INLINE void dSubtractVectorCross3(dReal *res, const dReal *a, const dReal *b)
00225 {
00226   dReal tmp[3];
00227   dCalcVectorCross3(tmp, a, b);
00228   dSubtractVectors3(res, res, tmp);
00229 }
00230 
00231 
00232 /*
00233  * set a 3x3 submatrix of A to a matrix such that submatrix(A)*b = a x b.
00234  * A is stored by rows, and has `skip' elements per row. the matrix is
00235  * assumed to be already zero, so this does not write zero elements!
00236  * if (plus,minus) is (+,-) then a positive version will be written.
00237  * if (plus,minus) is (-,+) then a negative version will be written.
00238  */
00239 
00240 PURE_INLINE void dSetCrossMatrixPlus(dReal *res, const dReal *a, unsigned skip)
00241 {
00242   const dReal a_0 = a[0], a_1 = a[1], a_2 = a[2];
00243   res[1] = -a_2;
00244   res[2] = +a_1;
00245   res[skip+0] = +a_2;
00246   res[skip+2] = -a_0;
00247   res[2*skip+0] = -a_1;
00248   res[2*skip+1] = +a_0;
00249 }
00250 
00251 PURE_INLINE void dSetCrossMatrixMinus(dReal *res, const dReal *a, unsigned skip)
00252 {
00253   const dReal a_0 = a[0], a_1 = a[1], a_2 = a[2];
00254   res[1] = +a_2;
00255   res[2] = -a_1;
00256   res[skip+0] = -a_2;
00257   res[skip+2] = +a_0;
00258   res[2*skip+0] = +a_1;
00259   res[2*skip+1] = -a_0;
00260 }
00261 
00262 
00263 /*
00264  * compute the distance between two 3D-vectors
00265  */
00266 
00267 PURE_INLINE dReal dCalcPointsDistance3(const dReal *a, const dReal *b)
00268 {
00269   dReal res;
00270   dReal tmp[3];
00271   dSubtractVectors3(tmp, a, b);
00272   res = dCalcVectorLength3(tmp);
00273   return res;
00274 }
00275 
00276 /*
00277  * special case matrix multiplication, with operator selection
00278  */
00279 
00280 PURE_INLINE void dMultiplyHelper0_331(dReal *res, const dReal *a, const dReal *b)
00281 {
00282   dReal res_0, res_1, res_2;
00283   res_0 = dCalcVectorDot3(a, b);
00284   res_1 = dCalcVectorDot3(a + 4, b);
00285   res_2 = dCalcVectorDot3(a + 8, b);
00286   // Only assign after all the calculations are over to avoid incurring memory aliasing
00287   res[0] = res_0; res[1] = res_1; res[2] = res_2;
00288 }
00289 
00290 PURE_INLINE void dMultiplyHelper1_331(dReal *res, const dReal *a, const dReal *b)
00291 {
00292   dReal res_0, res_1, res_2;
00293   res_0 = dCalcVectorDot3_41(a, b);
00294   res_1 = dCalcVectorDot3_41(a + 1, b);
00295   res_2 = dCalcVectorDot3_41(a + 2, b);
00296   // Only assign after all the calculations are over to avoid incurring memory aliasing
00297   res[0] = res_0; res[1] = res_1; res[2] = res_2;
00298 }
00299 
00300 PURE_INLINE void dMultiplyHelper0_133(dReal *res, const dReal *a, const dReal *b)
00301 {
00302   dMultiplyHelper1_331(res, b, a);
00303 }
00304 
00305 PURE_INLINE void dMultiplyHelper1_133(dReal *res, const dReal *a, const dReal *b)
00306 {
00307   dReal res_0, res_1, res_2;
00308   res_0 = dCalcVectorDot3_44(a, b);
00309   res_1 = dCalcVectorDot3_44(a + 1, b);
00310   res_2 = dCalcVectorDot3_44(a + 2, b);
00311   // Only assign after all the calculations are over to avoid incurring memory aliasing
00312   res[0] = res_0; res[1] = res_1; res[2] = res_2;
00313 }
00314 
00315 /* 
00316 Note: NEVER call any of these functions/macros with the same variable for A and C, 
00317 it is not equivalent to A*=B.
00318 */
00319 
00320 PURE_INLINE void dMultiply0_331(dReal *res, const dReal *a, const dReal *b)
00321 {
00322   dMultiplyHelper0_331(res, a, b);
00323 }
00324 
00325 PURE_INLINE void dMultiply1_331(dReal *res, const dReal *a, const dReal *b)
00326 {
00327   dMultiplyHelper1_331(res, a, b);
00328 }
00329 
00330 PURE_INLINE void dMultiply0_133(dReal *res, const dReal *a, const dReal *b)
00331 {
00332   dMultiplyHelper0_133(res, a, b);
00333 }
00334 
00335 PURE_INLINE void dMultiply0_333(dReal *res, const dReal *a, const dReal *b)
00336 {
00337   dMultiplyHelper0_133(res + 0, a + 0, b);
00338   dMultiplyHelper0_133(res + 4, a + 4, b);
00339   dMultiplyHelper0_133(res + 8, a + 8, b);
00340 }
00341 
00342 PURE_INLINE void dMultiply1_333(dReal *res, const dReal *a, const dReal *b)
00343 {
00344   dMultiplyHelper1_133(res + 0, b, a + 0);
00345   dMultiplyHelper1_133(res + 4, b, a + 1);
00346   dMultiplyHelper1_133(res + 8, b, a + 2);
00347 }
00348 
00349 PURE_INLINE void dMultiply2_333(dReal *res, const dReal *a, const dReal *b)
00350 {
00351   dMultiplyHelper0_331(res + 0, b, a + 0);
00352   dMultiplyHelper0_331(res + 4, b, a + 4);
00353   dMultiplyHelper0_331(res + 8, b, a + 8);
00354 }
00355 
00356 PURE_INLINE void dMultiplyAdd0_331(dReal *res, const dReal *a, const dReal *b)
00357 {
00358   dReal tmp[3];
00359   dMultiplyHelper0_331(tmp, a, b);
00360   dAddVectors3(res, res, tmp);
00361 }
00362 
00363 PURE_INLINE void dMultiplyAdd1_331(dReal *res, const dReal *a, const dReal *b)
00364 {
00365   dReal tmp[3];
00366   dMultiplyHelper1_331(tmp, a, b);
00367   dAddVectors3(res, res, tmp);
00368 }
00369 
00370 PURE_INLINE void dMultiplyAdd0_133(dReal *res, const dReal *a, const dReal *b)
00371 {
00372   dReal tmp[3];
00373   dMultiplyHelper0_133(tmp, a, b);
00374   dAddVectors3(res, res, tmp);
00375 }
00376 
00377 PURE_INLINE void dMultiplyAdd0_333(dReal *res, const dReal *a, const dReal *b)
00378 {
00379   dReal tmp[3];
00380   dMultiplyHelper0_133(tmp, a + 0, b);
00381   dAddVectors3(res+ 0, res + 0, tmp);
00382   dMultiplyHelper0_133(tmp, a + 4, b);
00383   dAddVectors3(res + 4, res + 4, tmp);
00384   dMultiplyHelper0_133(tmp, a + 8, b);
00385   dAddVectors3(res + 8, res + 8, tmp);
00386 }
00387 
00388 PURE_INLINE void dMultiplyAdd1_333(dReal *res, const dReal *a, const dReal *b)
00389 {
00390   dReal tmp[3];
00391   dMultiplyHelper1_133(tmp, b, a + 0);
00392   dAddVectors3(res + 0, res + 0, tmp);
00393   dMultiplyHelper1_133(tmp, b, a + 1);
00394   dAddVectors3(res + 4, res + 4, tmp);
00395   dMultiplyHelper1_133(tmp, b, a + 2);
00396   dAddVectors3(res + 8, res + 8, tmp);
00397 }
00398 
00399 PURE_INLINE void dMultiplyAdd2_333(dReal *res, const dReal *a, const dReal *b)
00400 {
00401   dReal tmp[3];
00402   dMultiplyHelper0_331(tmp, b, a + 0);
00403   dAddVectors3(res + 0, res + 0, tmp);
00404   dMultiplyHelper0_331(tmp, b, a + 4);
00405   dAddVectors3(res + 4, res + 4, tmp);
00406   dMultiplyHelper0_331(tmp, b, a + 8);
00407   dAddVectors3(res + 8, res + 8, tmp);
00408 }
00409 
00410 
00411 // Include legacy macros here
00412 #include <ode/odemath_legacy.h>
00413 
00414 
00415 #ifdef __cplusplus
00416 extern "C" {
00417 #endif
00418 
00419 /*
00420  * normalize 3x1 and 4x1 vectors (i.e. scale them to unit length)
00421  */
00422 
00423 // For DLL export
00424 ODE_API int  dSafeNormalize3 (dVector3 a);
00425 ODE_API int  dSafeNormalize4 (dVector4 a);
00426 ODE_API void dNormalize3 (dVector3 a); // Potentially asserts on zero vec
00427 ODE_API void dNormalize4 (dVector4 a); // Potentially asserts on zero vec
00428 
00429 #if defined(__ODE__)
00430 
00431 int  _dSafeNormalize3 (dVector3 a);
00432 int  _dSafeNormalize4 (dVector4 a);
00433 
00434 PURE_INLINE void _dNormalize3(dVector3 a)
00435 {
00436   int bNormalizationResult = _dSafeNormalize3(a);
00437   dIVERIFY(bNormalizationResult);
00438 }
00439 
00440 PURE_INLINE void _dNormalize4(dVector4 a)
00441 {
00442   int bNormalizationResult = _dSafeNormalize4(a);
00443   dIVERIFY(bNormalizationResult);
00444 }
00445 
00446 // For internal use
00447 #define dSafeNormalize3(a) _dSafeNormalize3(a)
00448 #define dSafeNormalize4(a) _dSafeNormalize4(a)
00449 #define dNormalize3(a) _dNormalize3(a)
00450 #define dNormalize4(a) _dNormalize4(a)
00451 
00452 #endif // defined(__ODE__)
00453 
00454 /*
00455  * given a unit length "normal" vector n, generate vectors p and q vectors
00456  * that are an orthonormal basis for the plane space perpendicular to n.
00457  * i.e. this makes p,q such that n,p,q are all perpendicular to each other.
00458  * q will equal n x p. if n is not unit length then p will be unit length but
00459  * q wont be.
00460  */
00461 
00462 ODE_API void dPlaneSpace (const dVector3 n, dVector3 p, dVector3 q);
00463 /* Makes sure the matrix is a proper rotation */
00464 ODE_API void dOrthogonalizeR(dMatrix3 m);
00465 
00466 
00467 
00468 #ifdef __cplusplus
00469 }
00470 #endif
00471 
00472 
00473 #endif