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/config.h>

    24 #include <ode/misc.h>

    25 #include <ode/matrix.h>

    26 #include <ode/ode.h>

    27 

    28 //****************************************************************************

    29 // random numbers

    30 

    31 EXPORT_C unsigned long dRand()

    32 {

    33   GetGlobalData()->seed = (1664525L*GetGlobalData()->seed + 1013904223L) & 0xffffffff;

    34   return GetGlobalData()->seed;

    35 }

    36 

    37 

    38 EXPORT_C unsigned long  dRandGetSeed()

    39 {

    40   return GetGlobalData()->seed;

    41 }

    42 

    43 

    44 EXPORT_C void dRandSetSeed (unsigned long s)

    45 {

    46   GetGlobalData()->seed = s;

    47 }

    48 

    49 

    50 EXPORT_C int dTestRand()

    51 {

    52   unsigned long oldseed = GetGlobalData()->seed;

    53   int ret = 1;

    54   GetGlobalData()->seed = 0;

    55   if (dRand() != 0x3c6ef35f || dRand() != 0x47502932 ||

    56       dRand() != 0xd1ccf6e9 || dRand() != 0xaaf95334 ||

    57       dRand() != 0x6252e503) ret = 0;

    58   GetGlobalData()->seed = oldseed;

    59   return ret;

    60 }

    61 

    62 

    63 // adam's all-int straightforward(?) dRandInt (0..n-1)

    64 EXPORT_C int dRandInt (int n)

    65 {

    66   // seems good; xor-fold and modulus

    67   const unsigned long un = n;

    68   unsigned long r = dRand();

    69   

    70   // note: probably more aggressive than it needs to be -- might be

    71   //       able to get away without one or two of the innermost branches.

    72   if (un <= 0x00010000UL) {

    73     r ^= (r >> 16);

    74     if (un <= 0x00000100UL) {

    75       r ^= (r >> 8);

    76       if (un <= 0x00000010UL) {

    77         r ^= (r >> 4);

    78         if (un <= 0x00000004UL) {

    79           r ^= (r >> 2);

    80           if (un <= 0x00000002UL) {

    81             r ^= (r >> 1);

    82           }

    83         }

    84       }

    85     }

    86   }

    87 

    88   return (int) (r % un);    

    89 }

    90 

    91 

    92 EXPORT_C dReal dRandReal()

    93 {

    94   return REAL(((dReal) dRand()) / ((dReal) 0xffffffff));

    95 }

    96 

    97 //****************************************************************************

    98 // matrix utility stuff

    99 

   100 EXPORT_C void dPrintMatrix (const dReal *A, int n, int m, char *fmt, FILE *f)

   101 {

   102   int i,j;

   103   int skip = dPAD(m);

   104   for (i=0; i<n; i++) {

   105     for (j=0; j<m; j++) fprintf (f,fmt,A[i*skip+j]);

   106     fprintf (f,"\n");

   107   }

   108 }

   109 

   110 

   111 EXPORT_C void dMakeRandomVector (dReal *A, int n, dReal range)

   112 {

   113   int i;

   114   for (i=0; i<n; i++) A[i] = dMUL((dMUL(dRandReal(),REAL(2.0))-REAL(1.0)),range);

   115 }

   116 

   117 

   118 EXPORT_C void dMakeRandomMatrix (dReal *A, int n, int m, dReal range)

   119 {

   120   int i,j;

   121   int skip = dPAD(m);

   122   dSetZero (A,n*skip);

   123   for (i=0; i<n; i++) {

   124     for (j=0; j<m; j++) A[i*skip+j] = dMUL((dMUL(dRandReal(),REAL(2.0))-REAL(1.0)),range);

   125   }

   126 }

   127 

   128 

   129 EXPORT_C void dClearUpperTriangle (dReal *A, int n)

   130 {

   131   int i,j;

   132   int skip = dPAD(n);

   133   for (i=0; i<n; i++) {

   134     for (j=i+1; j<n; j++) A[i*skip+j] = 0;

   135   }

   136 }

   137 

   138 

   139 EXPORT_C dReal dMaxDifference (const dReal *A, const dReal *B, int n, int m)

   140 {

   141   int i,j;

   142   int skip = dPAD(m);

   143   dReal diff,max;

   144   max = 0;

   145   for (i=0; i<n; i++) {

   146     for (j=0; j<m; j++) {

   147       diff = dFabs(A[i*skip+j] - B[i*skip+j]);

   148       if (diff > max) max = diff;

   149     }

   150   }

   151   return max;

   152 }

   153 

   154 

   155 EXPORT_C dReal dMaxDifferenceLowerTriangle (const dReal *A, const dReal *B, int n)

   156 {

   157   int i,j;

   158   int skip = dPAD(n);

   159   dReal diff,max;

   160   max = 0;

   161   for (i=0; i<n; i++) {

   162     for (j=0; j<=i; j++) {

   163       diff = dFabs(A[i*skip+j] - B[i*skip+j]);

   164       if (diff > max) max = diff;

   165     }

   166   }

   167   return max;

   168 }