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 "config.h"
  25 #include "odemath.h"
  26
  27
  28 #undef dSafeNormalize3
  29 #undef dSafeNormalize4
  30 #undef dNormalize3
  31 #undef dNormalize4
  32
  33 #undef dPlaneSpace
  34 #undef dOrthogonalizeR
  35
  36
  37 int  dSafeNormalize3 (dVector3 a)
  38 {
  39     return dxSafeNormalize3(a);
  40 }
  41
  42 int dSafeNormalize4 (dVector4 a)
  43 {
  44     return dxSafeNormalize4(a);
  45 }
  46
  47 void dNormalize3(dVector3 a)
  48 {
  49     dxNormalize3(a);
  50 }
  51
  52 void dNormalize4(dVector4 a)
  53 {
  54     dxNormalize4(a);
  55 }
  56
  57
  58 void dPlaneSpace(const dVector3 n, dVector3 p, dVector3 q)
  59 {
  60     return dxPlaneSpace(n, p, q);
  61 }
  62
  63 int dOrthogonalizeR(dMatrix3 m)
  64 {
  65     return dxOrthogonalizeR(m);
  66 }
  67
  68
  69 /*extern */
  70 bool dxCouldBeNormalized3(const dVector3 a)
  71 {
  72     dAASSERT (a);
  73
  74     bool ret = false;
  75
  76     for (unsigned axis = dV3E__AXES_MIN; axis != dV3E__AXES_MAX; ++axis) {
  77         if (a[axis] != REAL(0.0)) {
  78             ret = true;
  79             break;
  80         }
  81     }
  82
  83     return ret;
  84 }
  85
  86 // this may be called for vectors `a' with extremely small magnitude, for
  87 // example the result of a cross product on two nearly perpendicular vectors.
  88 // we must be robust to these small vectors. to prevent numerical error,
  89 // first find the component a[i] with the largest magnitude and then scale
  90 // all the components by 1/a[i]. then we can compute the length of `a' and
  91 // scale the components by 1/l. this has been verified to work with vectors
  92 // containing the smallest representable numbers.
  93
  94 /*extern */
  95 bool dxSafeNormalize3 (dVector3 a)
  96 {
  97     dAASSERT (a);
  98
  99     bool ret = false;
 100
 101     do {
 102         dReal abs_a0 = dFabs(a[dV3E_X]);
 103         dReal abs_a1 = dFabs(a[dV3E_Y]);
 104         dReal abs_a2 = dFabs(a[dV3E_Z]);
 105
 106         dVec3Element idx;
 107
 108         if (abs_a1 > abs_a0) {
 109             if (abs_a2 > abs_a1) { // abs_a2 is the largest
 110                 idx = dV3E_Z;
 111             }
 112             else {              // abs_a1 is the largest
 113                 idx = dV3E_Y;
 114             }
 115         }
 116         else if (abs_a2 > abs_a0) {// abs_a2 is the largest
 117             idx = dV3E_Z;
 118         }
 119         else {              // aa[0] might be the largest
 120             if (!(abs_a0 > REAL(0.0))) {
 121                 // if all a's are zero, this is where we'll end up.
 122                 // return the vector unchanged.
 123                 break;
 124             }
 125
 126             // abs_a0 is the largest
 127             idx = dV3E_X;
 128         }
 129
 130         if (idx == dV3E_X) {
 131             dReal aa0_recip = dRecip(abs_a0);
 132             dReal a1 = a[dV3E_Y] * aa0_recip;
 133             dReal a2 = a[dV3E_Z] * aa0_recip;
 134             dReal l = dRecipSqrt(REAL(1.0) + a1 * a1 + a2 * a2);
 135             a[dV3E_Y] = a1 * l;
 136             a[dV3E_Z] = a2 * l;
 137             a[dV3E_X] = dCopySign(l, a[dV3E_X]);
 138         }
 139         else if (idx == dV3E_Y) {
 140             dReal aa1_recip = dRecip(abs_a1);
 141             dReal a0 = a[dV3E_X] * aa1_recip;
 142             dReal a2 = a[dV3E_Z] * aa1_recip;
 143             dReal l = dRecipSqrt(REAL(1.0) + a0 * a0 + a2 * a2);
 144             a[dV3E_X] = a0 * l;
 145             a[dV3E_Z] = a2 * l;
 146             a[dV3E_Y] = dCopySign(l, a[dV3E_Y]);
 147         }
 148         else {
 149             dReal aa2_recip = dRecip(abs_a2);
 150             dReal a0 = a[dV3E_X] * aa2_recip;
 151             dReal a1 = a[dV3E_Y] * aa2_recip;
 152             dReal l = dRecipSqrt(REAL(1.0) + a0 * a0 + a1 * a1);
 153             a[dV3E_X] = a0 * l;
 154             a[dV3E_Y] = a1 * l;
 155             a[dV3E_Z] = dCopySign(l, a[dV3E_Z]);
 156         }
 157
 158         ret = true;
 159     }
 160     while (false);
 161
 162     return ret;
 163 }
 164
 165 /* OLD VERSION */
 166 /*
 167 void dNormalize3 (dVector3 a)
 168 {
 169     dIASSERT (a);
 170     dReal l = dCalcVectorDot3(a,a);
 171     if (l > 0) {
 172         l = dRecipSqrt(l);
 173         a[0] *= l;
 174         a[1] *= l;
 175         a[2] *= l;
 176     }
 177     else {
 178         a[0] = 1;
 179         a[1] = 0;
 180         a[2] = 0;
 181     }
 182 }
 183 */
 184
 185 /*extern */
 186 bool dxCouldBeNormalized4(const dVector4 a)
 187 {
 188     dAASSERT (a);
 189
 190     bool ret = false;
 191
 192     for (unsigned axis = dV4E__MIN; axis != dV4E__MAX; ++axis) {
 193         if (a[axis] != REAL(0.0)) {
 194             ret = true;
 195             break;
 196         }
 197     }
 198
 199     return ret;
 200 }
 201
 202 /*extern */
 203 bool dxSafeNormalize4 (dVector4 a)
 204 {
 205     dAASSERT (a);
 206
 207     bool ret = false;
 208
 209     dReal l = a[dV4E_X] * a[dV4E_X] + a[dV4E_Y] * a[dV4E_Y] + a[dV4E_Z] * a[dV4E_Z] + a[dV4E_O] * a[dV4E_O];
 210     if (l > 0) {
 211         l = dRecipSqrt(l);
 212         a[dV4E_X] *= l;
 213         a[dV4E_Y] *= l;
 214         a[dV4E_Z] *= l;
 215         a[dV4E_O] *= l;
 216
 217         ret = true;
 218     }
 219
 220     return ret;
 221 }
 222
 223
 224 void dxPlaneSpace (const dVector3 n, dVector3 p, dVector3 q)
 225 {
 226     dAASSERT (n && p && q);
 227     if (dFabs(n[2]) > M_SQRT1_2) {
 228         // choose p in y-z plane
 229         dReal a = n[1]*n[1] + n[2]*n[2];
 230         dReal k = dRecipSqrt (a);
 231         p[0] = 0;
 232         p[1] = -n[2]*k;
 233         p[2] = n[1]*k;
 234         // set q = n x p
 235         q[0] = a*k;
 236         q[1] = -n[0]*p[2];
 237         q[2] = n[0]*p[1];
 238     }
 239     else {
 240         // choose p in x-y plane
 241         dReal a = n[0]*n[0] + n[1]*n[1];
 242         dReal k = dRecipSqrt (a);
 243         p[0] = -n[1]*k;
 244         p[1] = n[0]*k;
 245         p[2] = 0;
 246         // set q = n x p
 247         q[0] = -n[2]*p[1];
 248         q[1] = n[2]*p[0];
 249         q[2] = a*k;
 250     }
 251 }
 252
 253
 254 /*
 255 * This takes what is supposed to be a rotation matrix,
 256 * and make sure it is correct.
 257 * Note: this operates on rows, not columns, because for rotations
 258 * both ways give equivalent results.
 259 */
 260 bool dxOrthogonalizeR(dMatrix3 m)
 261 {
 262     bool ret = false;
 263
 264     do {
 265         if (!dxCouldBeNormalized3(m + dM3E__X_MIN)) {
 266             break;
 267         }
 268
 269         dReal n0 = dCalcVectorLengthSquare3(m + dM3E__X_MIN);
 270
 271         dVector3 row2_store;
 272         dReal *row2 = m + dM3E__Y_MIN;
 273         // project row[0] on row[1], should be zero
 274         dReal proj = dCalcVectorDot3(m + dM3E__X_MIN, m + dM3E__Y_MIN);
 275         if (proj != 0) {
 276             // Gram-Schmidt step on row[1]
 277             dReal proj_div_n0 = proj / n0;
 278             row2_store[dV3E_X] = m[dM3E__Y_MIN + dV3E_X] - proj_div_n0 * m[dM3E__X_MIN + dV3E_X] ;
 279             row2_store[dV3E_Y] = m[dM3E__Y_MIN + dV3E_Y] - proj_div_n0 * m[dM3E__X_MIN + dV3E_Y];
 280             row2_store[dV3E_Z] = m[dM3E__Y_MIN + dV3E_Z] - proj_div_n0 * m[dM3E__X_MIN + dV3E_Z];
 281             row2 = row2_store;
 282         }
 283
 284         if (!dxCouldBeNormalized3(row2)) {
 285             break;
 286         }
 287
 288         if (n0 != REAL(1.0)) {
 289             bool row0_norm_fault = !dxSafeNormalize3(m + dM3E__X_MIN);
 290             dIVERIFY(!row0_norm_fault);
 291         }
 292
 293         dReal n1 = dCalcVectorLengthSquare3(row2);
 294         if (n1 != REAL(1.0)) {
 295             bool row1_norm_fault = !dxSafeNormalize3(row2);
 296             dICHECK(!row1_norm_fault);
 297         }
 298
 299         dIASSERT(dFabs(dCalcVectorDot3(m + dM3E__X_MIN, row2)) < 1e-6);
 300
 301         /* just overwrite row[2], this makes sure the matrix is not
 302         a reflection */
 303         dCalcVectorCross3(m + dM3E__Z_MIN, m + dM3E__X_MIN, row2);
 304
 305         m[dM3E_XPAD] = m[dM3E_YPAD] = m[dM3E_ZPAD] = 0;
 306
 307         ret = true;
 308     }
 309     while (false);
 310
 311     return ret;
 312 }