extern/ode/dist/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/config.h>
  24 #include <ode/misc.h>
  25 #include <ode/matrix.h>
  26
  27 //****************************************************************************
  28 // random numbers
  29
  30 static unsigned long seed = 0;
  31
  32 unsigned long dRand()
  33 {
  34   seed = (1664525L*seed + 1013904223L) & 0xffffffff;
  35   return seed;
  36 }
  37
  38
  39 unsigned long  dRandGetSeed()
  40 {
  41   return seed;
  42 }
  43
  44
  45 void dRandSetSeed (unsigned long s)
  46 {
  47   seed = s;
  48 }
  49
  50
  51 int dTestRand()
  52 {
  53   unsigned long oldseed = seed;
  54   int ret = 1;
  55   seed = 0;
  56   if (dRand() != 0x3c6ef35f || dRand() != 0x47502932 ||
  57       dRand() != 0xd1ccf6e9 || dRand() != 0xaaf95334 ||
  58       dRand() != 0x6252e503) ret = 0;
  59   seed = oldseed;
  60   return ret;
  61 }
  62
  63
  64 int dRandInt (int n)
  65 {
  66   double a = double(n) / 4294967296.0;
  67   return (int) (double(dRand()) * a);
  68 }
  69
  70
  71 dReal dRandReal()
  72 {
  73   return ((dReal) dRand()) / ((dReal) 0xffffffff);
  74 }
  75
  76 //****************************************************************************
  77 // matrix utility stuff
  78
  79 void dPrintMatrix (dReal *A, int n, int m, char *fmt, FILE *f)
  80 {
  81   int i,j;
  82   int skip = dPAD(m);
  83   for (i=0; i<n; i++) {
  84     for (j=0; j<m; j++) fprintf (f,fmt,A[i*skip+j]);
  85     fprintf (f,"\n");
  86   }
  87 }
  88
  89
  90 void dMakeRandomVector (dReal *A, int n, dReal range)
  91 {
  92   int i;
  93   for (i=0; i<n; i++) A[i] = (dRandReal()*REAL(2.0)-REAL(1.0))*range;
  94 }
  95
  96
  97 void dMakeRandomMatrix (dReal *A, int n, int m, dReal range)
  98 {
  99   int i,j;
 100   int skip = dPAD(m);
 101   dSetZero (A,n*skip);
 102   for (i=0; i<n; i++) {
 103     for (j=0; j<m; j++) A[i*skip+j] = (dRandReal()*REAL(2.0)-REAL(1.0))*range;
 104   }
 105 }
 106
 107
 108 void dClearUpperTriangle (dReal *A, int n)
 109 {
 110   int i,j;
 111   int skip = dPAD(n);
 112   for (i=0; i<n; i++) {
 113     for (j=i+1; j<n; j++) A[i*skip+j] = 0;
 114   }
 115 }
 116
 117
 118 dReal dMaxDifference (const dReal *A, const dReal *B, int n, int m)
 119 {
 120   int i,j;
 121   int skip = dPAD(m);
 122   dReal diff,max;
 123   max = 0;
 124   for (i=0; i<n; i++) {
 125     for (j=0; j<m; j++) {
 126       diff = dFabs(A[i*skip+j] - B[i*skip+j]);
 127       if (diff > max) max = diff;
 128     }
 129   }
 130   return max;
 131 }
 132
 133
 134 dReal dMaxDifferenceLowerTriangle (const dReal *A, const dReal *B, int n)
 135 {
 136   int i,j;
 137   int skip = dPAD(n);
 138   dReal diff,max;
 139   max = 0;
 140   for (i=0; i<n; i++) {
 141     for (j=0; j<=i; j++) {
 142       diff = dFabs(A[i*skip+j] - B[i*skip+j]);
 143       if (diff > max) max = diff;
 144     }
 145   }
 146   return max;
 147 }