ode/src/misc.cpp

   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 #include <ode/odeconfig.h>
  24 #include <ode/misc.h>
  25 #include "config.h"
  26 #include "matrix.h"
  27 #include "error.h"
  28 #include "odeou.h"
  29
  30 //****************************************************************************
  31 // random numbers
  32
  33 static volatile duint32 seed = 0;
  34
  35 unsigned long dRand()
  36 {
  37     duint32 origSeed, newSeed;
  38 #if !dTHREADING_INTF_DISABLED
  39     do {
  40 #endif
  41         origSeed = seed;
  42         newSeed = ((duint32)1664525 * origSeed + (duint32)1013904223) & (duint32)0xffffffff;
  43 #if dTHREADING_INTF_DISABLED
  44         seed = newSeed;
  45 #else
  46     } while (!AtomicCompareExchange((volatile atomicord32 *)&seed, origSeed, newSeed));
  47 #endif
  48     return newSeed;
  49 }
  50
  51
  52 unsigned long  dRandGetSeed()
  53 {
  54     return seed;
  55 }
  56
  57
  58 void dRandSetSeed (unsigned long s)
  59 {
  60     seed = s;
  61 }
  62
  63
  64 int dTestRand()
  65 {
  66     unsigned long oldseed = seed;
  67     int ret = 1;
  68     seed = 0;
  69     if (dRand() != 0x3c6ef35f || dRand() != 0x47502932 ||
  70         dRand() != 0xd1ccf6e9 || dRand() != 0xaaf95334 ||
  71         dRand() != 0x6252e503) ret = 0;
  72     seed = oldseed;
  73     return ret;
  74 }
  75
  76
  77 // adam's all-int straightforward(?) dRandInt (0..n-1)
  78 int dRandInt (int n)
  79 {
  80     int result;
  81     // Since there is no memory barrier macro in ODE assign via volatile variable
  82     // to prevent compiler reusing seed as value of `r'
  83     volatile unsigned long raw_r = dRand();
  84     duint32 r = (duint32)raw_r;
  85
  86     duint32 un = n;
  87     dIASSERT(sizeof(n) == sizeof(un));
  88
  89     // note: probably more aggressive than it needs to be -- might be
  90     //       able to get away without one or two of the innermost branches.
  91     // if (un <= 0x00010000UL) {
  92     //     r ^= (r >> 16);
  93     //     if (un <= 0x00000100UL) {
  94     //         r ^= (r >> 8);
  95     //         if (un <= 0x00000010UL) {
  96     //             r ^= (r >> 4);
  97     //             if (un <= 0x00000004UL) {
  98     //                 r ^= (r >> 2);
  99     //                 if (un <= 0x00000002UL) {
 100     //                     r ^= (r >> 1);
 101     //                 }
 102     //             }
 103     //         }
 104     //     }
 105     // }
 106     // Optimized version of above
 107     if (un <= (duint32)0x00000010) {
 108         r ^= (r >> 16);
 109         r ^= (r >> 8);
 110         r ^= (r >> 4);
 111         if (un <= (duint32)0x00000002) {
 112             r ^= (r >> 2);
 113             r ^= (r >> 1);
 114             result = (r/* & (duint32)0x01*/) & (un >> 1);
 115         } else {
 116             if (un <= (duint32)0x00000004) {
 117                 r ^= (r >> 2);
 118                 result = ((r & (duint32)0x03) * un) >> 2;
 119             } else {
 120                 result = ((r & (duint32)0x0F) * un) >> 4;
 121             }
 122         }
 123     } else {
 124         if (un <= (duint32)0x00000100) {
 125             r ^= (r >> 16);
 126             r ^= (r >> 8);
 127             result = ((r & (duint32)0xFF) * un) >> 8;
 128         } else {
 129             if (un <= (duint32)0x00010000) {
 130                 r ^= (r >> 16);
 131                 result = ((r & (duint32)0xFFFF) * un) >> 16;
 132             } else {
 133                 result = (int)(((duint64)r * un) >> 32);
 134             }
 135         }
 136     }
 137
 138     return result;
 139 }
 140
 141
 142 dReal dRandReal()
 143 {
 144     return (dReal)(((double) dRand()) / ((double) 0xffffffff));
 145 }
 146
 147 //****************************************************************************
 148 // matrix utility stuff
 149
 150 void dPrintMatrix (const dReal *A, int n, int m, const char *fmt, FILE *f)
 151 {
 152     int skip = dPAD(m);
 153     const dReal *Arow = A;
 154     for (int i=0; i<n; Arow+=skip, ++i) {
 155         for (int j=0; j<m; ++j) fprintf (f,fmt,Arow[j]);
 156         fprintf (f,"\n");
 157     }
 158 }
 159
 160
 161 void dMakeRandomVector (dReal *A, int n, dReal range)
 162 {
 163     int i;
 164     for (i=0; i<n; i++) A[i] = (dRandReal()*REAL(2.0)-REAL(1.0))*range;
 165 }
 166
 167
 168 void dMakeRandomMatrix (dReal *A, int n, int m, dReal range)
 169 {
 170     int skip = dPAD(m);
 171     //  dSetZero (A,n*skip);
 172     dReal *Arow = A;
 173     for (int i=0; i<n; Arow+=skip, ++i) {
 174         for (int j=0; j<m; ++j) Arow[j] = (dRandReal()*REAL(2.0)-REAL(1.0))*range;
 175     }
 176 }
 177
 178
 179 void dClearUpperTriangle (dReal *A, int n)
 180 {
 181     int skip = dPAD(n);
 182     dReal *Arow = A;
 183     for (int i=0; i<n; Arow+=skip, ++i) {
 184         for (int j=i+1; j<n; ++j) Arow[j] = 0;
 185     }
 186 }
 187
 188
 189 dReal dMaxDifference (const dReal *A, const dReal *B, int n, int m)
 190 {
 191     int skip = dPAD(m);
 192     dReal max = REAL(0.0);
 193     const dReal *Arow = A, *Brow = B;
 194     for (int i=0; i<n; Arow+=skip, Brow +=skip, ++i) {
 195         for (int j=0; j<m; ++j) {
 196             dReal diff = dFabs(Arow[j] - Brow[j]);
 197             if (diff > max) max = diff;
 198         }
 199     }
 200     return max;
 201 }
 202
 203
 204 dReal dMaxDifferenceLowerTriangle (const dReal *A, const dReal *B, int n)
 205 {
 206     int skip = dPAD(n);
 207     dReal max = REAL(0.0);
 208     const dReal *Arow = A, *Brow = B;
 209     for (int i=0; i<n; Arow+=skip, Brow+=skip, ++i) {
 210         for (int j=0; j<=i; ++j) {
 211             dReal diff = dFabs(Arow[j] - Brow[j]);
 212             if (diff > max) max = diff;
 213         }
 214     }
 215     return max;
 216 }
 217