Open Dynamics Engine
|
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