From c5fc66ee58f2c60f2d226868bb1cf5b91badaf53 Mon Sep 17 00:00:00 2001 From: sanine Date: Sat, 1 Oct 2022 20:59:36 -0500 Subject: add ode --- libs/ode-0.16.1/ode/src/odemath.cpp | 312 ++++++++++++++++++++++++++++++++++++ 1 file changed, 312 insertions(+) create mode 100644 libs/ode-0.16.1/ode/src/odemath.cpp (limited to 'libs/ode-0.16.1/ode/src/odemath.cpp') diff --git a/libs/ode-0.16.1/ode/src/odemath.cpp b/libs/ode-0.16.1/ode/src/odemath.cpp new file mode 100644 index 0000000..5e69b9b --- /dev/null +++ b/libs/ode-0.16.1/ode/src/odemath.cpp @@ -0,0 +1,312 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include +#include "config.h" +#include "odemath.h" + + +#undef dSafeNormalize3 +#undef dSafeNormalize4 +#undef dNormalize3 +#undef dNormalize4 + +#undef dPlaneSpace +#undef dOrthogonalizeR + + +int dSafeNormalize3 (dVector3 a) +{ + return dxSafeNormalize3(a); +} + +int dSafeNormalize4 (dVector4 a) +{ + return dxSafeNormalize4(a); +} + +void dNormalize3(dVector3 a) +{ + dxNormalize3(a); +} + +void dNormalize4(dVector4 a) +{ + dxNormalize4(a); +} + + +void dPlaneSpace(const dVector3 n, dVector3 p, dVector3 q) +{ + return dxPlaneSpace(n, p, q); +} + +int dOrthogonalizeR(dMatrix3 m) +{ + return dxOrthogonalizeR(m); +} + + +/*extern */ +bool dxCouldBeNormalized3(const dVector3 a) +{ + dAASSERT (a); + + bool ret = false; + + for (unsigned axis = dV3E__AXES_MIN; axis != dV3E__AXES_MAX; ++axis) { + if (a[axis] != REAL(0.0)) { + ret = true; + break; + } + } + + return ret; +} + +// this may be called for vectors `a' with extremely small magnitude, for +// example the result of a cross product on two nearly perpendicular vectors. +// we must be robust to these small vectors. to prevent numerical error, +// first find the component a[i] with the largest magnitude and then scale +// all the components by 1/a[i]. then we can compute the length of `a' and +// scale the components by 1/l. this has been verified to work with vectors +// containing the smallest representable numbers. + +/*extern */ +bool dxSafeNormalize3 (dVector3 a) +{ + dAASSERT (a); + + bool ret = false; + + do { + dReal abs_a0 = dFabs(a[dV3E_X]); + dReal abs_a1 = dFabs(a[dV3E_Y]); + dReal abs_a2 = dFabs(a[dV3E_Z]); + + dVec3Element idx; + + if (abs_a1 > abs_a0) { + if (abs_a2 > abs_a1) { // abs_a2 is the largest + idx = dV3E_Z; + } + else { // abs_a1 is the largest + idx = dV3E_Y; + } + } + else if (abs_a2 > abs_a0) {// abs_a2 is the largest + idx = dV3E_Z; + } + else { // aa[0] might be the largest + if (!(abs_a0 > REAL(0.0))) { + // if all a's are zero, this is where we'll end up. + // return the vector unchanged. + break; + } + + // abs_a0 is the largest + idx = dV3E_X; + } + + if (idx == dV3E_X) { + dReal aa0_recip = dRecip(abs_a0); + dReal a1 = a[dV3E_Y] * aa0_recip; + dReal a2 = a[dV3E_Z] * aa0_recip; + dReal l = dRecipSqrt(REAL(1.0) + a1 * a1 + a2 * a2); + a[dV3E_Y] = a1 * l; + a[dV3E_Z] = a2 * l; + a[dV3E_X] = dCopySign(l, a[dV3E_X]); + } + else if (idx == dV3E_Y) { + dReal aa1_recip = dRecip(abs_a1); + dReal a0 = a[dV3E_X] * aa1_recip; + dReal a2 = a[dV3E_Z] * aa1_recip; + dReal l = dRecipSqrt(REAL(1.0) + a0 * a0 + a2 * a2); + a[dV3E_X] = a0 * l; + a[dV3E_Z] = a2 * l; + a[dV3E_Y] = dCopySign(l, a[dV3E_Y]); + } + else { + dReal aa2_recip = dRecip(abs_a2); + dReal a0 = a[dV3E_X] * aa2_recip; + dReal a1 = a[dV3E_Y] * aa2_recip; + dReal l = dRecipSqrt(REAL(1.0) + a0 * a0 + a1 * a1); + a[dV3E_X] = a0 * l; + a[dV3E_Y] = a1 * l; + a[dV3E_Z] = dCopySign(l, a[dV3E_Z]); + } + + ret = true; + } + while (false); + + return ret; +} + +/* OLD VERSION */ +/* +void dNormalize3 (dVector3 a) +{ + dIASSERT (a); + dReal l = dCalcVectorDot3(a,a); + if (l > 0) { + l = dRecipSqrt(l); + a[0] *= l; + a[1] *= l; + a[2] *= l; + } + else { + a[0] = 1; + a[1] = 0; + a[2] = 0; + } +} +*/ + +/*extern */ +bool dxCouldBeNormalized4(const dVector4 a) +{ + dAASSERT (a); + + bool ret = false; + + for (unsigned axis = dV4E__MIN; axis != dV4E__MAX; ++axis) { + if (a[axis] != REAL(0.0)) { + ret = true; + break; + } + } + + return ret; +} + +/*extern */ +bool dxSafeNormalize4 (dVector4 a) +{ + dAASSERT (a); + + bool ret = false; + + 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]; + if (l > 0) { + l = dRecipSqrt(l); + a[dV4E_X] *= l; + a[dV4E_Y] *= l; + a[dV4E_Z] *= l; + a[dV4E_O] *= l; + + ret = true; + } + + return ret; +} + + +void dxPlaneSpace (const dVector3 n, dVector3 p, dVector3 q) +{ + dAASSERT (n && p && q); + if (dFabs(n[2]) > M_SQRT1_2) { + // choose p in y-z plane + dReal a = n[1]*n[1] + n[2]*n[2]; + dReal k = dRecipSqrt (a); + p[0] = 0; + p[1] = -n[2]*k; + p[2] = n[1]*k; + // set q = n x p + q[0] = a*k; + q[1] = -n[0]*p[2]; + q[2] = n[0]*p[1]; + } + else { + // choose p in x-y plane + dReal a = n[0]*n[0] + n[1]*n[1]; + dReal k = dRecipSqrt (a); + p[0] = -n[1]*k; + p[1] = n[0]*k; + p[2] = 0; + // set q = n x p + q[0] = -n[2]*p[1]; + q[1] = n[2]*p[0]; + q[2] = a*k; + } +} + + +/* +* This takes what is supposed to be a rotation matrix, +* and make sure it is correct. +* Note: this operates on rows, not columns, because for rotations +* both ways give equivalent results. +*/ +bool dxOrthogonalizeR(dMatrix3 m) +{ + bool ret = false; + + do { + if (!dxCouldBeNormalized3(m + dM3E__X_MIN)) { + break; + } + + dReal n0 = dCalcVectorLengthSquare3(m + dM3E__X_MIN); + + dVector3 row2_store; + dReal *row2 = m + dM3E__Y_MIN; + // project row[0] on row[1], should be zero + dReal proj = dCalcVectorDot3(m + dM3E__X_MIN, m + dM3E__Y_MIN); + if (proj != 0) { + // Gram-Schmidt step on row[1] + dReal proj_div_n0 = proj / n0; + row2_store[dV3E_X] = m[dM3E__Y_MIN + dV3E_X] - proj_div_n0 * m[dM3E__X_MIN + dV3E_X] ; + row2_store[dV3E_Y] = m[dM3E__Y_MIN + dV3E_Y] - proj_div_n0 * m[dM3E__X_MIN + dV3E_Y]; + row2_store[dV3E_Z] = m[dM3E__Y_MIN + dV3E_Z] - proj_div_n0 * m[dM3E__X_MIN + dV3E_Z]; + row2 = row2_store; + } + + if (!dxCouldBeNormalized3(row2)) { + break; + } + + if (n0 != REAL(1.0)) { + bool row0_norm_fault = !dxSafeNormalize3(m + dM3E__X_MIN); + dIVERIFY(!row0_norm_fault); + } + + dReal n1 = dCalcVectorLengthSquare3(row2); + if (n1 != REAL(1.0)) { + bool row1_norm_fault = !dxSafeNormalize3(row2); + dICHECK(!row1_norm_fault); + } + + dIASSERT(dFabs(dCalcVectorDot3(m + dM3E__X_MIN, row2)) < 1e-6); + + /* just overwrite row[2], this makes sure the matrix is not + a reflection */ + dCalcVectorCross3(m + dM3E__Z_MIN, m + dM3E__X_MIN, row2); + + m[dM3E_XPAD] = m[dM3E_YPAD] = m[dM3E_ZPAD] = 0; + + ret = true; + } + while (false); + + return ret; +} -- cgit v1.2.1