From 6c2e2ac2e135f89d1f248113c93afcb71e6c598c Mon Sep 17 00:00:00 2001 From: David Rose Date: Thu, 20 Jun 2002 16:48:07 +0000 Subject: [PATCH] add paranoid-hpr-quat --- panda/src/linmath/config_linmath.cxx | 26 +++++++++ panda/src/linmath/config_linmath.h | 4 ++ panda/src/linmath/lquaternion_src.cxx | 78 +++++++++++++++++++-------- 3 files changed, 86 insertions(+), 22 deletions(-) diff --git a/panda/src/linmath/config_linmath.cxx b/panda/src/linmath/config_linmath.cxx index 6380c83b7b..70693ab37e 100644 --- a/panda/src/linmath/config_linmath.cxx +++ b/panda/src/linmath/config_linmath.cxx @@ -26,6 +26,32 @@ Configure(config_linmath); NotifyCategoryDef(linmath, ""); ConfigureFn(config_linmath) { + init_liblinmath(); +} + +// Set this true to doublecheck the quaternion-hpr compose and +// decompose operations against the quaternion-matrix and matrix-hpr +// operations. This only has effect if NDEBUG is not defined. + +// The default is true for now. +const bool paranoid_hpr_quat = config_linmath.GetBool("paranoid-hpr-quat", true); + +//////////////////////////////////////////////////////////////////// +// Function: init_liblinmath +// Description: Initializes the library. This must be called at +// least once before any of the functions or classes in +// this library can be used. Normally it will be +// called by the static initializers and need not be +// called explicitly, but special cases exist. +//////////////////////////////////////////////////////////////////// +void +init_liblinmath() { + static bool initialized = false; + if (initialized) { + return; + } + initialized = true; + LVecBase2f::init_type(); LVecBase3f::init_type(); LVecBase4f::init_type(); diff --git a/panda/src/linmath/config_linmath.h b/panda/src/linmath/config_linmath.h index c05e57889d..7a8d61fab4 100644 --- a/panda/src/linmath/config_linmath.h +++ b/panda/src/linmath/config_linmath.h @@ -24,4 +24,8 @@ NotifyCategoryDecl(linmath, EXPCL_PANDA, EXPTP_PANDA); +extern const bool paranoid_hpr_quat; + +extern EXPCL_PANDA void init_liblinmath(); + #endif diff --git a/panda/src/linmath/lquaternion_src.cxx b/panda/src/linmath/lquaternion_src.cxx index a52cf15595..982afe0368 100644 --- a/panda/src/linmath/lquaternion_src.cxx +++ b/panda/src/linmath/lquaternion_src.cxx @@ -16,6 +16,9 @@ // //////////////////////////////////////////////////////////////////// +#include "config_linmath.h" +#include "compose_matrix_src.h" + TypeHandle FLOATNAME(LQuaternion)::_type_handle; const FLOATNAME(LQuaternion) FLOATNAME(LQuaternion)::_ident_quat = @@ -85,23 +88,39 @@ void FLOATNAME(LQuaternion):: set_hpr(const FLOATNAME(LVecBase3) &hpr) { FLOATNAME(LQuaternion) quat_h, quat_p, quat_r; - FLOATNAME(LVector3) v = FLOATNAME(LVector3)::up(); - FLOATTYPE a = deg_2_rad(hpr[0] * 0.5); - FLOATTYPE s,c; + FLOATNAME(LVector3) v; + FLOATTYPE a, s, c; - csincos(a,&s,&c); + v = FLOATNAME(LVector3)::up(); + a = deg_2_rad(hpr[0] * 0.5f); + csincos(a, &s, &c); quat_h.set(c, v[0] * s, v[1] * s, v[2] * s); v = FLOATNAME(LVector3)::right(); - a = deg_2_rad(hpr[1] * 0.5); - csincos(a,&s,&c); + a = deg_2_rad(hpr[1] * 0.5f); + csincos(a, &s, &c); s = csin(a); quat_p.set(c, v[0] * s, v[1] * s, v[2] * s); v = FLOATNAME(LVector3)::forward(); - a = deg_2_rad(hpr[2] * 0.5); - csincos(a,&s,&c); + a = deg_2_rad(hpr[2] * 0.5f); + csincos(a, &s, &c); quat_r.set(c, v[0] * s, v[1] * s, v[2] * s); (*this) = quat_h * quat_p * quat_r; + +#ifndef NDEBUG + if (paranoid_hpr_quat) { + FLOATNAME(LMatrix3) mat; + compose_matrix(mat, FLOATNAME(LVecBase3)(1.0f, 1.0f, 1.0f), hpr); + FLOATNAME(LQuaternion) compare; + compare.set_from_matrix(mat); + if (!compare.almost_equal(*this)) { + linmath_cat.warning() + << "hpr-to-quat of " << hpr << " computed " << *this + << " instead of " << compare << "\n"; + (*this) = compare; + } + } +#endif // NDEBUG } //////////////////////////////////////////////////////////////////// @@ -112,9 +131,9 @@ set_hpr(const FLOATNAME(LVecBase3) &hpr) { //////////////////////////////////////////////////////////////////// FLOATNAME(LVecBase3) FLOATNAME(LQuaternion):: get_hpr() const { - FLOATTYPE heading, pitch, roll; + FLOATNAME(LVecBase3) hpr; FLOATTYPE N = (_v.data[0] * _v.data[0]) + (_v.data[1] * _v.data[1]) + (_v.data[2] * _v.data[2]) + (_v.data[3] * _v.data[3]); - FLOATTYPE s = (N == 0.) ? 0. : (2. / N); + FLOATTYPE s = (N == 0.0f) ? 0.0f : (2.0f / N); FLOATTYPE xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz, c1, c2, c3, c4; FLOATTYPE cr, sr, cp, sp, ch, sh; @@ -123,36 +142,51 @@ get_hpr() const { xx = _v.data[1] * xs; xy = _v.data[1] * ys; xz = _v.data[1] * zs; yy = _v.data[2] * ys; yz = _v.data[2] * zs; zz = _v.data[3] * zs; c1 = xz - wy; - c2 = 1. - (xx + yy); - c3 = 1. - (yy + zz); + c2 = 1.0f - (xx + yy); + c3 = 1.0f - (yy + zz); c4 = xy + wz; - if (c1 == 0.) { // (roll = 0 or 180) or (pitch = +/- 90 - if (c2 >= 0.) { - roll = 0.; + if (c1 == 0.0f) { // (roll = 0 or 180) or (pitch = +/- 90 + if (c2 >= 0.0f) { + hpr[2] = 0.0f; ch = c3; sh = c4; cp = c2; } else { - roll = 180.; + hpr[2] = 180.0f; ch = -c3; sh = -c4; cp = -c2; } } else { // this should work all the time, but the above saves some trig operations - roll = catan2(-c1, c2); - csincos(roll,&sr,&cr); - roll = rad_2_deg(roll); + FLOATTYPE roll = catan2(-c1, c2); + csincos(roll, &sr, &cr); + hpr[2] = rad_2_deg(roll); ch = (cr * c3) + (sr * (xz + wy)); sh = (cr * c4) + (sr * (yz - wx)); cp = (cr * c2) - (sr * c1); } sp = yz + wx; - heading = rad_2_deg(catan2(sh, ch)); - pitch = rad_2_deg(catan2(sp, cp)); + hpr[0] = rad_2_deg(catan2(sh, ch)); + hpr[1] = rad_2_deg(catan2(sp, cp)); - return FLOATNAME(LVecBase3)(heading, pitch, roll); +#ifndef NDEBUG + if (paranoid_hpr_quat) { + FLOATNAME(LMatrix3) mat; + extract_to_matrix(mat); + FLOATNAME(LVecBase3) scale, compare_hpr; + decompose_matrix(mat, scale, compare_hpr); + if (!compare_hpr.almost_equal(hpr)) { + linmath_cat.warning() + << "quat-to-hpr of " << *this << " computed " << hpr << " instead of " + << compare_hpr << "\n"; + hpr = compare_hpr; + } + } +#endif // NDEBUG + + return hpr; } ////////////////////////////////////////////////////////////////////