diff options
Diffstat (limited to 'libs/ode-0.16.1/ode/src/misc.cpp')
| -rw-r--r-- | libs/ode-0.16.1/ode/src/misc.cpp | 217 |
1 files changed, 217 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/ode/src/misc.cpp b/libs/ode-0.16.1/ode/src/misc.cpp new file mode 100644 index 0000000..e63a029 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/misc.cpp @@ -0,0 +1,217 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <ode/odeconfig.h> +#include <ode/misc.h> +#include "config.h" +#include "matrix.h" +#include "error.h" +#include "odeou.h" + +//**************************************************************************** +// random numbers + +static volatile duint32 seed = 0; + +unsigned long dRand() +{ + duint32 origSeed, newSeed; +#if !dTHREADING_INTF_DISABLED + do { +#endif + origSeed = seed; + newSeed = ((duint32)1664525 * origSeed + (duint32)1013904223) & (duint32)0xffffffff; +#if dTHREADING_INTF_DISABLED + seed = newSeed; +#else + } while (!AtomicCompareExchange((volatile atomicord32 *)&seed, origSeed, newSeed)); +#endif + return newSeed; +} + + +unsigned long dRandGetSeed() +{ + return seed; +} + + +void dRandSetSeed (unsigned long s) +{ + seed = s; +} + + +int dTestRand() +{ + unsigned long oldseed = seed; + int ret = 1; + seed = 0; + if (dRand() != 0x3c6ef35f || dRand() != 0x47502932 || + dRand() != 0xd1ccf6e9 || dRand() != 0xaaf95334 || + dRand() != 0x6252e503) ret = 0; + seed = oldseed; + return ret; +} + + +// adam's all-int straightforward(?) dRandInt (0..n-1) +int dRandInt (int n) +{ + int result; + // Since there is no memory barrier macro in ODE assign via volatile variable + // to prevent compiler reusing seed as value of `r' + volatile unsigned long raw_r = dRand(); + duint32 r = (duint32)raw_r; + + duint32 un = n; + dIASSERT(sizeof(n) == sizeof(un)); + + // note: probably more aggressive than it needs to be -- might be + // able to get away without one or two of the innermost branches. + // if (un <= 0x00010000UL) { + // r ^= (r >> 16); + // if (un <= 0x00000100UL) { + // r ^= (r >> 8); + // if (un <= 0x00000010UL) { + // r ^= (r >> 4); + // if (un <= 0x00000004UL) { + // r ^= (r >> 2); + // if (un <= 0x00000002UL) { + // r ^= (r >> 1); + // } + // } + // } + // } + // } + // Optimized version of above + if (un <= (duint32)0x00000010) { + r ^= (r >> 16); + r ^= (r >> 8); + r ^= (r >> 4); + if (un <= (duint32)0x00000002) { + r ^= (r >> 2); + r ^= (r >> 1); + result = (r/* & (duint32)0x01*/) & (un >> 1); + } else { + if (un <= (duint32)0x00000004) { + r ^= (r >> 2); + result = ((r & (duint32)0x03) * un) >> 2; + } else { + result = ((r & (duint32)0x0F) * un) >> 4; + } + } + } else { + if (un <= (duint32)0x00000100) { + r ^= (r >> 16); + r ^= (r >> 8); + result = ((r & (duint32)0xFF) * un) >> 8; + } else { + if (un <= (duint32)0x00010000) { + r ^= (r >> 16); + result = ((r & (duint32)0xFFFF) * un) >> 16; + } else { + result = (int)(((duint64)r * un) >> 32); + } + } + } + + return result; +} + + +dReal dRandReal() +{ + return (dReal)(((double) dRand()) / ((double) 0xffffffff)); +} + +//**************************************************************************** +// matrix utility stuff + +void dPrintMatrix (const dReal *A, int n, int m, const char *fmt, FILE *f) +{ + int skip = dPAD(m); + const dReal *Arow = A; + for (int i=0; i<n; Arow+=skip, ++i) { + for (int j=0; j<m; ++j) fprintf (f,fmt,Arow[j]); + fprintf (f,"\n"); + } +} + + +void dMakeRandomVector (dReal *A, int n, dReal range) +{ + int i; + for (i=0; i<n; i++) A[i] = (dRandReal()*REAL(2.0)-REAL(1.0))*range; +} + + +void dMakeRandomMatrix (dReal *A, int n, int m, dReal range) +{ + int skip = dPAD(m); + // dSetZero (A,n*skip); + dReal *Arow = A; + for (int i=0; i<n; Arow+=skip, ++i) { + for (int j=0; j<m; ++j) Arow[j] = (dRandReal()*REAL(2.0)-REAL(1.0))*range; + } +} + + +void dClearUpperTriangle (dReal *A, int n) +{ + int skip = dPAD(n); + dReal *Arow = A; + for (int i=0; i<n; Arow+=skip, ++i) { + for (int j=i+1; j<n; ++j) Arow[j] = 0; + } +} + + +dReal dMaxDifference (const dReal *A, const dReal *B, int n, int m) +{ + int skip = dPAD(m); + dReal max = REAL(0.0); + const dReal *Arow = A, *Brow = B; + for (int i=0; i<n; Arow+=skip, Brow +=skip, ++i) { + for (int j=0; j<m; ++j) { + dReal diff = dFabs(Arow[j] - Brow[j]); + if (diff > max) max = diff; + } + } + return max; +} + + +dReal dMaxDifferenceLowerTriangle (const dReal *A, const dReal *B, int n) +{ + int skip = dPAD(n); + dReal max = REAL(0.0); + const dReal *Arow = A, *Brow = B; + for (int i=0; i<n; Arow+=skip, Brow+=skip, ++i) { + for (int j=0; j<=i; ++j) { + dReal diff = dFabs(Arow[j] - Brow[j]); + if (diff > max) max = diff; + } + } + return max; +} + |