|
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 #ifdef __GNUC__ 00029 #define PURE_INLINE extern inline 00030 #else 00031 #define PURE_INLINE inline 00032 #endif 00033 00034 /* 00035 * macro to access elements i,j in an NxM matrix A, independent of the 00036 * matrix storage convention. 00037 */ 00038 #define dACCESS33(A,i,j) ((A)[(i)*4+(j)]) 00039 00040 /* 00041 * Macro to test for valid floating point values 00042 */ 00043 #define dVALIDVEC3(v) (!(dIsNan(v[0]) || dIsNan(v[1]) || dIsNan(v[2]))) 00044 #define dVALIDVEC4(v) (!(dIsNan(v[0]) || dIsNan(v[1]) || dIsNan(v[2]) || dIsNan(v[3]))) 00045 #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]))) 00046 #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]) )) 00047 00048 00049 00050 /* 00051 * General purpose vector operations with other vectors or constants. 00052 */ 00053 00054 #define dOP(a,op,b,c) do { \ 00055 (a)[0] = ((b)[0]) op ((c)[0]); \ 00056 (a)[1] = ((b)[1]) op ((c)[1]); \ 00057 (a)[2] = ((b)[2]) op ((c)[2]); \ 00058 } while (0) 00059 #define dOPC(a,op,b,c) do { \ 00060 (a)[0] = ((b)[0]) op (c); \ 00061 (a)[1] = ((b)[1]) op (c); \ 00062 (a)[2] = ((b)[2]) op (c); \ 00063 } while (0) 00064 #define dOPE(a,op,b) do {\ 00065 (a)[0] op ((b)[0]); \ 00066 (a)[1] op ((b)[1]); \ 00067 (a)[2] op ((b)[2]); \ 00068 } while (0) 00069 #define dOPEC(a,op,c) do { \ 00070 (a)[0] op (c); \ 00071 (a)[1] op (c); \ 00072 (a)[2] op (c); \ 00073 } while (0) 00074 00081 #define dOPE2(a,op1,b,op2,c) do { \ 00082 (a)[0] op1 ((b)[0]) op2 ((c)[0]); \ 00083 (a)[1] op1 ((b)[1]) op2 ((c)[1]); \ 00084 (a)[2] op1 ((b)[2]) op2 ((c)[2]); \ 00085 } while (0) 00086 00087 /* 00088 * Length, and squared length helpers. dLENGTH returns the length of a dVector3. 00089 * dLENGTHSQUARED return the squared length of a dVector3. 00090 */ 00091 00092 #define dLENGTHSQUARED(a) (((a)[0])*((a)[0]) + ((a)[1])*((a)[1]) + ((a)[2])*((a)[2])) 00093 00094 #ifdef __cplusplus 00095 00096 PURE_INLINE dReal dLENGTH (const dReal *a) { return dSqrt(dLENGTHSQUARED(a)); } 00097 00098 #else 00099 00100 #define dLENGTH(a) ( dSqrt( ((a)[0])*((a)[0]) + ((a)[1])*((a)[1]) + ((a)[2])*((a)[2]) ) ) 00101 00102 #endif /* __cplusplus */ 00103 00104 00105 00106 00107 00108 /* 00109 * 3-way dot product. dDOTpq means that elements of `a' and `b' are spaced 00110 * p and q indexes apart respectively. dDOT() means dDOT11. 00111 * in C++ we could use function templates to get all the versions of these 00112 * functions - but on some compilers this will result in sub-optimal code. 00113 */ 00114 00115 #define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)]) 00116 00117 #ifdef __cplusplus 00118 00119 PURE_INLINE dReal dDOT (const dReal *a, const dReal *b) { return dDOTpq(a,b,1,1); } 00120 PURE_INLINE dReal dDOT13 (const dReal *a, const dReal *b) { return dDOTpq(a,b,1,3); } 00121 PURE_INLINE dReal dDOT31 (const dReal *a, const dReal *b) { return dDOTpq(a,b,3,1); } 00122 PURE_INLINE dReal dDOT33 (const dReal *a, const dReal *b) { return dDOTpq(a,b,3,3); } 00123 PURE_INLINE dReal dDOT14 (const dReal *a, const dReal *b) { return dDOTpq(a,b,1,4); } 00124 PURE_INLINE dReal dDOT41 (const dReal *a, const dReal *b) { return dDOTpq(a,b,4,1); } 00125 PURE_INLINE dReal dDOT44 (const dReal *a, const dReal *b) { return dDOTpq(a,b,4,4); } 00126 00127 #else 00128 00129 #define dDOT(a,b) dDOTpq(a,b,1,1) 00130 #define dDOT13(a,b) dDOTpq(a,b,1,3) 00131 #define dDOT31(a,b) dDOTpq(a,b,3,1) 00132 #define dDOT33(a,b) dDOTpq(a,b,3,3) 00133 #define dDOT14(a,b) dDOTpq(a,b,1,4) 00134 #define dDOT41(a,b) dDOTpq(a,b,4,1) 00135 #define dDOT44(a,b) dDOTpq(a,b,4,4) 00136 00137 #endif /* __cplusplus */ 00138 00139 00140 /* 00141 * cross product, set a = b x c. dCROSSpqr means that elements of `a', `b' 00142 * and `c' are spaced p, q and r indexes apart respectively. 00143 * dCROSS() means dCROSS111. `op' is normally `=', but you can set it to 00144 * +=, -= etc to get other effects. 00145 */ 00146 00147 #define dCROSS(a,op,b,c) \ 00148 do { \ 00149 (a)[0] op ((b)[1]*(c)[2] - (b)[2]*(c)[1]); \ 00150 (a)[1] op ((b)[2]*(c)[0] - (b)[0]*(c)[2]); \ 00151 (a)[2] op ((b)[0]*(c)[1] - (b)[1]*(c)[0]); \ 00152 } while(0) 00153 #define dCROSSpqr(a,op,b,c,p,q,r) \ 00154 do { \ 00155 (a)[ 0] op ((b)[ q]*(c)[2*r] - (b)[2*q]*(c)[ r]); \ 00156 (a)[ p] op ((b)[2*q]*(c)[ 0] - (b)[ 0]*(c)[2*r]); \ 00157 (a)[2*p] op ((b)[ 0]*(c)[ r] - (b)[ q]*(c)[ 0]); \ 00158 } while(0) 00159 #define dCROSS114(a,op,b,c) dCROSSpqr(a,op,b,c,1,1,4) 00160 #define dCROSS141(a,op,b,c) dCROSSpqr(a,op,b,c,1,4,1) 00161 #define dCROSS144(a,op,b,c) dCROSSpqr(a,op,b,c,1,4,4) 00162 #define dCROSS411(a,op,b,c) dCROSSpqr(a,op,b,c,4,1,1) 00163 #define dCROSS414(a,op,b,c) dCROSSpqr(a,op,b,c,4,1,4) 00164 #define dCROSS441(a,op,b,c) dCROSSpqr(a,op,b,c,4,4,1) 00165 #define dCROSS444(a,op,b,c) dCROSSpqr(a,op,b,c,4,4,4) 00166 00167 00168 /* 00169 * set a 3x3 submatrix of A to a matrix such that submatrix(A)*b = a x b. 00170 * A is stored by rows, and has `skip' elements per row. the matrix is 00171 * assumed to be already zero, so this does not write zero elements! 00172 * if (plus,minus) is (+,-) then a positive version will be written. 00173 * if (plus,minus) is (-,+) then a negative version will be written. 00174 */ 00175 00176 #define dCROSSMAT(A,a,skip,plus,minus) \ 00177 do { \ 00178 (A)[1] = minus (a)[2]; \ 00179 (A)[2] = plus (a)[1]; \ 00180 (A)[(skip)+0] = plus (a)[2]; \ 00181 (A)[(skip)+2] = minus (a)[0]; \ 00182 (A)[2*(skip)+0] = minus (a)[1]; \ 00183 (A)[2*(skip)+1] = plus (a)[0]; \ 00184 } while(0) 00185 00186 00187 /* 00188 * compute the distance between two 3D-vectors 00189 */ 00190 00191 #ifdef __cplusplus 00192 PURE_INLINE dReal dDISTANCE (const dVector3 a, const dVector3 b) 00193 { return dSqrt( (a[0]-b[0])*(a[0]-b[0]) + (a[1]-b[1])*(a[1]-b[1]) + (a[2]-b[2])*(a[2]-b[2]) ); } 00194 #else 00195 #define dDISTANCE(a,b) \ 00196 (dSqrt( ((a)[0]-(b)[0])*((a)[0]-(b)[0]) + ((a)[1]-(b)[1])*((a)[1]-(b)[1]) + ((a)[2]-(b)[2])*((a)[2]-(b)[2]) )) 00197 #endif 00198 00199 00200 /* 00201 * special case matrix multipication, with operator selection 00202 */ 00203 00204 #define dMULTIPLYOP0_331(A,op,B,C) \ 00205 do { \ 00206 (A)[0] op dDOT((B),(C)); \ 00207 (A)[1] op dDOT((B+4),(C)); \ 00208 (A)[2] op dDOT((B+8),(C)); \ 00209 } while(0) 00210 #define dMULTIPLYOP1_331(A,op,B,C) \ 00211 do { \ 00212 (A)[0] op dDOT41((B),(C)); \ 00213 (A)[1] op dDOT41((B+1),(C)); \ 00214 (A)[2] op dDOT41((B+2),(C)); \ 00215 } while(0) 00216 #define dMULTIPLYOP0_133(A,op,B,C) \ 00217 do { \ 00218 (A)[0] op dDOT14((B),(C)); \ 00219 (A)[1] op dDOT14((B),(C+1)); \ 00220 (A)[2] op dDOT14((B),(C+2)); \ 00221 } while(0) 00222 #define dMULTIPLYOP0_333(A,op,B,C) \ 00223 do { \ 00224 (A)[0] op dDOT14((B),(C)); \ 00225 (A)[1] op dDOT14((B),(C+1)); \ 00226 (A)[2] op dDOT14((B),(C+2)); \ 00227 (A)[4] op dDOT14((B+4),(C)); \ 00228 (A)[5] op dDOT14((B+4),(C+1)); \ 00229 (A)[6] op dDOT14((B+4),(C+2)); \ 00230 (A)[8] op dDOT14((B+8),(C)); \ 00231 (A)[9] op dDOT14((B+8),(C+1)); \ 00232 (A)[10] op dDOT14((B+8),(C+2)); \ 00233 } while(0) 00234 #define dMULTIPLYOP1_333(A,op,B,C) \ 00235 do { \ 00236 (A)[0] op dDOT44((B),(C)); \ 00237 (A)[1] op dDOT44((B),(C+1)); \ 00238 (A)[2] op dDOT44((B),(C+2)); \ 00239 (A)[4] op dDOT44((B+1),(C)); \ 00240 (A)[5] op dDOT44((B+1),(C+1)); \ 00241 (A)[6] op dDOT44((B+1),(C+2)); \ 00242 (A)[8] op dDOT44((B+2),(C)); \ 00243 (A)[9] op dDOT44((B+2),(C+1)); \ 00244 (A)[10] op dDOT44((B+2),(C+2)); \ 00245 } while(0) 00246 #define dMULTIPLYOP2_333(A,op,B,C) \ 00247 do { \ 00248 (A)[0] op dDOT((B),(C)); \ 00249 (A)[1] op dDOT((B),(C+4)); \ 00250 (A)[2] op dDOT((B),(C+8)); \ 00251 (A)[4] op dDOT((B+4),(C)); \ 00252 (A)[5] op dDOT((B+4),(C+4)); \ 00253 (A)[6] op dDOT((B+4),(C+8)); \ 00254 (A)[8] op dDOT((B+8),(C)); \ 00255 (A)[9] op dDOT((B+8),(C+4)); \ 00256 (A)[10] op dDOT((B+8),(C+8)); \ 00257 } while(0) 00258 00259 #ifdef __cplusplus 00260 00261 #define DECL template <class TA, class TB, class TC> PURE_INLINE void 00262 00263 /* 00264 Note: NEVER call any of these functions/macros with the same variable for A and C, 00265 it is not equivalent to A*=B. 00266 */ 00267 00268 DECL dMULTIPLY0_331(TA *A, const TB *B, const TC *C) { dMULTIPLYOP0_331(A,=,B,C); } 00269 DECL dMULTIPLY1_331(TA *A, const TB *B, const TC *C) { dMULTIPLYOP1_331(A,=,B,C); } 00270 DECL dMULTIPLY0_133(TA *A, const TB *B, const TC *C) { dMULTIPLYOP0_133(A,=,B,C); } 00271 DECL dMULTIPLY0_333(TA *A, const TB *B, const TC *C) { dMULTIPLYOP0_333(A,=,B,C); } 00272 DECL dMULTIPLY1_333(TA *A, const TB *B, const TC *C) { dMULTIPLYOP1_333(A,=,B,C); } 00273 DECL dMULTIPLY2_333(TA *A, const TB *B, const TC *C) { dMULTIPLYOP2_333(A,=,B,C); } 00274 00275 DECL dMULTIPLYADD0_331(TA *A, const TB *B, const TC *C) { dMULTIPLYOP0_331(A,+=,B,C); } 00276 DECL dMULTIPLYADD1_331(TA *A, const TB *B, const TC *C) { dMULTIPLYOP1_331(A,+=,B,C); } 00277 DECL dMULTIPLYADD0_133(TA *A, const TB *B, const TC *C) { dMULTIPLYOP0_133(A,+=,B,C); } 00278 DECL dMULTIPLYADD0_333(TA *A, const TB *B, const TC *C) { dMULTIPLYOP0_333(A,+=,B,C); } 00279 DECL dMULTIPLYADD1_333(TA *A, const TB *B, const TC *C) { dMULTIPLYOP1_333(A,+=,B,C); } 00280 DECL dMULTIPLYADD2_333(TA *A, const TB *B, const TC *C) { dMULTIPLYOP2_333(A,+=,B,C); } 00281 00282 #undef DECL 00283 00284 #else 00285 00286 #define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C) 00287 #define dMULTIPLY1_331(A,B,C) dMULTIPLYOP1_331(A,=,B,C) 00288 #define dMULTIPLY0_133(A,B,C) dMULTIPLYOP0_133(A,=,B,C) 00289 #define dMULTIPLY0_333(A,B,C) dMULTIPLYOP0_333(A,=,B,C) 00290 #define dMULTIPLY1_333(A,B,C) dMULTIPLYOP1_333(A,=,B,C) 00291 #define dMULTIPLY2_333(A,B,C) dMULTIPLYOP2_333(A,=,B,C) 00292 00293 #define dMULTIPLYADD0_331(A,B,C) dMULTIPLYOP0_331(A,+=,B,C) 00294 #define dMULTIPLYADD1_331(A,B,C) dMULTIPLYOP1_331(A,+=,B,C) 00295 #define dMULTIPLYADD0_133(A,B,C) dMULTIPLYOP0_133(A,+=,B,C) 00296 #define dMULTIPLYADD0_333(A,B,C) dMULTIPLYOP0_333(A,+=,B,C) 00297 #define dMULTIPLYADD1_333(A,B,C) dMULTIPLYOP1_333(A,+=,B,C) 00298 #define dMULTIPLYADD2_333(A,B,C) dMULTIPLYOP2_333(A,+=,B,C) 00299 00300 #endif 00301 00302 00303 #ifdef __cplusplus 00304 extern "C" { 00305 #endif 00306 00307 /* 00308 * normalize 3x1 and 4x1 vectors (i.e. scale them to unit length) 00309 */ 00310 00311 #if defined(__ODE__) 00312 00313 int _dSafeNormalize3 (dVector3 a); 00314 int _dSafeNormalize4 (dVector4 a); 00315 00316 static __inline void _dNormalize3(dVector3 a) 00317 { 00318 int bNormalizationResult = _dSafeNormalize3(a); 00319 dIASSERT(bNormalizationResult); 00320 dVARIABLEUSED(bNormalizationResult); 00321 } 00322 00323 static __inline void _dNormalize4(dVector4 a) 00324 { 00325 int bNormalizationResult = _dSafeNormalize4(a); 00326 dIASSERT(bNormalizationResult); 00327 dVARIABLEUSED(bNormalizationResult); 00328 } 00329 00330 #endif // defined(__ODE__) 00331 00332 // For DLL export 00333 ODE_API int dSafeNormalize3 (dVector3 a); 00334 ODE_API int dSafeNormalize4 (dVector4 a); 00335 ODE_API void dNormalize3 (dVector3 a); // Potentially asserts on zero vec 00336 ODE_API void dNormalize4 (dVector4 a); // Potentially asserts on zero vec 00337 00338 #if defined(__ODE__) 00339 00340 // For internal use 00341 #define dSafeNormalize3(a) _dSafeNormalize3(a) 00342 #define dSafeNormalize4(a) _dSafeNormalize4(a) 00343 #define dNormalize3(a) _dNormalize3(a) 00344 #define dNormalize4(a) _dNormalize4(a) 00345 00346 #endif // defined(__ODE__) 00347 00348 /* 00349 * given a unit length "normal" vector n, generate vectors p and q vectors 00350 * that are an orthonormal basis for the plane space perpendicular to n. 00351 * i.e. this makes p,q such that n,p,q are all perpendicular to each other. 00352 * q will equal n x p. if n is not unit length then p will be unit length but 00353 * q wont be. 00354 */ 00355 00356 ODE_API void dPlaneSpace (const dVector3 n, dVector3 p, dVector3 q); 00357 /* Makes sure the matrix is a proper rotation */ 00358 ODE_API void dOrthogonalizeR(dMatrix3 m); 00359 00360 00361 00362 #ifdef __cplusplus 00363 } 00364 #endif 00365 00366 #endif
1.7.3