ode/src/odemath.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/common.h>
  24 #include <ode/odemath.h>
  25
  26 // get some math functions under windows
  27 #ifdef WIN32
  28 #include <float.h>
  29 #ifndef CYGWIN                  // added by andy for cygwin
  30 #undef copysign
  31 #define copysign(a,b) ((dReal)_copysign(a,b))
  32 #endif                          // added by andy for cygwin
  33 #endif
  34
  35 #undef dSafeNormalize3
  36 #undef dSafeNormalize4
  37 #undef dNormalize3
  38 #undef dNormalize4
  39
  40
  41 // this may be called for vectors `a' with extremely small magnitude, for
  42 // example the result of a cross product on two nearly perpendicular vectors.
  43 // we must be robust to these small vectors. to prevent numerical error,
  44 // first find the component a[i] with the largest magnitude and then scale
  45 // all the components by 1/a[i]. then we can compute the length of `a' and
  46 // scale the components by 1/l. this has been verified to work with vectors
  47 // containing the smallest representable numbers.
  48
  49 int _dSafeNormalize3 (dVector3 a)
  50 {
  51   dAASSERT (a);
  52
  53   int idx;
  54   dReal aa[3], l;
  55
  56   aa[0] = dFabs(a[0]);
  57   aa[1] = dFabs(a[1]);
  58   aa[2] = dFabs(a[2]);
  59   if (aa[1] > aa[0]) {
  60     if (aa[2] > aa[1]) { // aa[2] is largest
  61       idx = 2;
  62     }
  63     else {              // aa[1] is largest
  64       idx = 1;
  65     }
  66   }
  67   else {
  68     if (aa[2] > aa[0]) {// aa[2] is largest
  69       idx = 2;
  70     }
  71     else {              // aa[0] might be the largest
  72       if (aa[0] <= 0) { // aa[0] might is largest
  73         a[0] = 1;       // if all a's are zero, this is where we'll end up.
  74         a[1] = 0;       // return a default unit length vector.
  75         a[2] = 0;
  76         return 0;
  77       }
  78       else {
  79         idx = 0;
  80       }
  81     }
  82   }
  83
  84   a[0] /= aa[idx];
  85   a[1] /= aa[idx];
  86   a[2] /= aa[idx];
  87   l = dRecipSqrt (a[0]*a[0] + a[1]*a[1] + a[2]*a[2]);
  88   a[0] *= l;
  89   a[1] *= l;
  90   a[2] *= l;
  91
  92   return 1;
  93 }
  94
  95 /* OLD VERSION */
  96 /*
  97 void dNormalize3 (dVector3 a)
  98 {
  99   dIASSERT (a);
 100   dReal l = dDOT(a,a);
 101   if (l > 0) {
 102     l = dRecipSqrt(l);
 103     a[0] *= l;
 104     a[1] *= l;
 105     a[2] *= l;
 106   }
 107   else {
 108     a[0] = 1;
 109     a[1] = 0;
 110     a[2] = 0;
 111   }
 112 }
 113 */
 114
 115 int  dSafeNormalize3 (dVector3 a)
 116 {
 117         return _dSafeNormalize3(a);
 118 }
 119
 120 void dNormalize3(dVector3 a)
 121 {
 122         _dNormalize3(a);
 123 }
 124
 125
 126 int _dSafeNormalize4 (dVector4 a)
 127 {
 128   dAASSERT (a);
 129   dReal l = dDOT(a,a)+a[3]*a[3];
 130   if (l > 0) {
 131     l = dRecipSqrt(l);
 132     a[0] *= l;
 133     a[1] *= l;
 134     a[2] *= l;
 135     a[3] *= l;
 136         return 1;
 137   }
 138   else {
 139     a[0] = 1;
 140     a[1] = 0;
 141     a[2] = 0;
 142     a[3] = 0;
 143     return 0;
 144   }
 145 }
 146
 147 int  dSafeNormalize4 (dVector4 a)
 148 {
 149         return _dSafeNormalize4(a);
 150 }
 151
 152 void dNormalize4(dVector4 a)
 153 {
 154         _dNormalize4(a);
 155 }
 156
 157
 158 void dPlaneSpace (const dVector3 n, dVector3 p, dVector3 q)
 159 {
 160   dAASSERT (n && p && q);
 161   if (dFabs(n[2]) > M_SQRT1_2) {
 162     // choose p in y-z plane
 163     dReal a = n[1]*n[1] + n[2]*n[2];
 164     dReal k = dRecipSqrt (a);
 165     p[0] = 0;
 166     p[1] = -n[2]*k;
 167     p[2] = n[1]*k;
 168     // set q = n x p
 169     q[0] = a*k;
 170     q[1] = -n[0]*p[2];
 171     q[2] = n[0]*p[1];
 172   }
 173   else {
 174     // choose p in x-y plane
 175     dReal a = n[0]*n[0] + n[1]*n[1];
 176     dReal k = dRecipSqrt (a);
 177     p[0] = -n[1]*k;
 178     p[1] = n[0]*k;
 179     p[2] = 0;
 180     // set q = n x p
 181     q[0] = -n[2]*p[1];
 182     q[1] = n[2]*p[0];
 183     q[2] = a*k;
 184   }
 185 }