diff --git a/panda/src/linmath/compose_matrix.cxx b/panda/src/linmath/compose_matrix.cxx index 88388f202f..eba0bd39b3 100644 --- a/panda/src/linmath/compose_matrix.cxx +++ b/panda/src/linmath/compose_matrix.cxx @@ -20,13 +20,6 @@ #include "config_linmath.h" #include "compose_matrix.h" -Configure(config_linmath_2); -ConfigureFn(config_linmath_2) { -} - - -static const bool temp_hpr_fix = config_linmath_2.GetBool("temp-hpr-fix", false); - #include "fltnames.h" #include "compose_matrix_src.cxx" diff --git a/panda/src/linmath/config_linmath.cxx b/panda/src/linmath/config_linmath.cxx index 70693ab37e..89ebef9b4e 100644 --- a/panda/src/linmath/config_linmath.cxx +++ b/panda/src/linmath/config_linmath.cxx @@ -36,6 +36,15 @@ ConfigureFn(config_linmath) { // The default is true for now. const bool paranoid_hpr_quat = config_linmath.GetBool("paranoid-hpr-quat", true); +// Set this true to compute hpr's correctly. Presently, we apply +// these in the wrong order, and roll is backwards relative to the +// other two. But we can't globally fix this because some of our old +// tools, most notably egg-optchar, depend on the old broken behavior. +// Until we are able to rewrite these tools into the new system, we +// must keep the old behavior; setting this switch lets you use the +// new, correct behavior but you don't get animated characters. +const bool temp_hpr_fix = config_linmath.GetBool("temp-hpr-fix", false); + //////////////////////////////////////////////////////////////////// // Function: init_liblinmath // Description: Initializes the library. This must be called at diff --git a/panda/src/linmath/lquaternion_src.I b/panda/src/linmath/lquaternion_src.I index 80768c5f31..128cd6770f 100644 --- a/panda/src/linmath/lquaternion_src.I +++ b/panda/src/linmath/lquaternion_src.I @@ -31,8 +31,8 @@ FLOATNAME(LQuaternion)() { // Description: //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LQuaternion):: -FLOATNAME(LQuaternion)(const FLOATNAME(LQuaternion) &c) : - FLOATNAME(LVecBase4)(c) +FLOATNAME(LQuaternion)(const FLOATNAME(LVecBase4) ©) : + FLOATNAME(LVecBase4)(copy) { } @@ -46,9 +46,25 @@ FLOATNAME(LQuaternion)(FLOATTYPE r, FLOATTYPE i, FLOATTYPE j, FLOATTYPE k) { set(r, i, j, k); } +//////////////////////////////////////////////////////////////////// +// Function: LQuaternion::xform +// Access: Published +// Description: Transforms a 3-d vector by the indicated rotation +//////////////////////////////////////////////////////////////////// +INLINE_LINMATH FLOATNAME(LVecBase3) FLOATNAME(LQuaternion):: +xform(const FLOATNAME(LVecBase3) &v) const { + FLOATNAME(LQuaternion) v_quat(0.0f, v[0], v[1], v[2]); + + FLOATNAME(LQuaternion) inv; + inv.invert_from(*this); + v_quat = (*this) * v_quat * inv; + + return FLOATNAME(LVecBase3)(v_quat[1], v_quat[2], v_quat[3]); +} + //////////////////////////////////////////////////////////////////// // Function: LQuaternion::multiply -// Access: protected +// Access: Published // Description: actual multiply call (non virtual) //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LQuaternion) FLOATNAME(LQuaternion):: @@ -61,13 +77,23 @@ multiply(const FLOATNAME(LQuaternion)& rhs) const { return FLOATNAME(LQuaternion)(r, i , j, k); } +//////////////////////////////////////////////////////////////////// +// Function: LQuaternion::unary - +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE_LINMATH FLOATNAME(LQuaternion) FLOATNAME(LQuaternion):: +operator - () const { + return FLOATNAME(LVecBase4)::operator - (); +} + //////////////////////////////////////////////////////////////////// // Function: LQuaternion::Multiply Operator // Access: public // Description: //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LQuaternion) FLOATNAME(LQuaternion):: -operator *(const FLOATNAME(LQuaternion)& c) { +operator *(const FLOATNAME(LQuaternion)& c) const { return multiply(c); } @@ -278,12 +304,7 @@ normalize() { //////////////////////////////////////////////////////////////////// INLINE_LINMATH bool FLOATNAME(LQuaternion):: invert_from(const FLOATNAME(LQuaternion) &other) { - FLOATTYPE norm = 1.0f / (other.dot(other)); - set(other[0] * norm, - -other[1] * norm, - -other[2] * norm, - -other[3] * norm); - + set(other._v.v._0, -other._v.v._1, -other._v.v._2, -other._v.v._3); return true; } @@ -296,12 +317,9 @@ invert_from(const FLOATNAME(LQuaternion) &other) { //////////////////////////////////////////////////////////////////// INLINE_LINMATH bool FLOATNAME(LQuaternion):: invert_in_place() { - FLOATTYPE norm = 1.0f / ((*this).dot(*this)); - set((*this)[0] * norm, - -(*this)[1] * norm, - -(*this)[2] * norm, - -(*this)[3] * norm); - + _v.v._1 = -_v.v._1; + _v.v._2 = -_v.v._2; + _v.v._3 = -_v.v._3; return true; } diff --git a/panda/src/linmath/lquaternion_src.cxx b/panda/src/linmath/lquaternion_src.cxx index 89a9fdb21d..400fca176f 100644 --- a/panda/src/linmath/lquaternion_src.cxx +++ b/panda/src/linmath/lquaternion_src.cxx @@ -108,13 +108,18 @@ set_hpr(const FLOATNAME(LVecBase3) &hpr) { (*this) = quat_h * quat_p * quat_r; + if (!temp_hpr_fix) { + // Compute the old, broken hpr. + (*this) = invert(quat_r) * quat_h * quat_p; + } + #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)) { + if (!compare.almost_equal(*this) && !compare.almost_equal(-(*this))) { linmath_cat.warning() << "hpr-to-quat of " << hpr << " computed " << *this << " instead of " << compare << "\n"; @@ -132,6 +137,19 @@ set_hpr(const FLOATNAME(LVecBase3) &hpr) { //////////////////////////////////////////////////////////////////// FLOATNAME(LVecBase3) FLOATNAME(LQuaternion):: get_hpr() const { + if (!temp_hpr_fix) { + // With the old, broken hpr code in place, just go through the + // existing matrix decomposition code. Not particularly speedy, + // but I don't want to bother with working out how to do it + // directly for code that hopefully won't need to last much + // longer. + FLOATNAME(LMatrix3) mat; + extract_to_matrix(mat); + FLOATNAME(LVecBase3) scale, hpr; + decompose_matrix(mat, scale, hpr); + return hpr; + } + 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.0f) ? 0.0f : (2.0f / N); @@ -147,7 +165,7 @@ get_hpr() const { c3 = 1.0f - (yy + zz); c4 = xy + wz; - if (c1 == 0.0f) { // (roll = 0 or 180) or (pitch = +/- 90 + if (c1 == 0.0f) { // (roll = 0 or 180) or (pitch = +/- 90) if (c2 >= 0.0f) { hpr[2] = 0.0f; ch = c3; diff --git a/panda/src/linmath/lquaternion_src.h b/panda/src/linmath/lquaternion_src.h index 29d2efef67..5b039bb728 100644 --- a/panda/src/linmath/lquaternion_src.h +++ b/panda/src/linmath/lquaternion_src.h @@ -21,18 +21,22 @@ // Description : This is the base quaternion class //////////////////////////////////////////////////////////////////// class EXPCL_PANDA FLOATNAME(LQuaternion) : public FLOATNAME(LVecBase4) { -protected: - INLINE_LINMATH FLOATNAME(LQuaternion) - multiply(const FLOATNAME(LQuaternion)&) const; - PUBLISHED: INLINE_LINMATH FLOATNAME(LQuaternion)(); - INLINE_LINMATH FLOATNAME(LQuaternion)(const FLOATNAME(LQuaternion) &); + INLINE_LINMATH FLOATNAME(LQuaternion)(const FLOATNAME(LVecBase4) ©); INLINE_LINMATH FLOATNAME(LQuaternion)(FLOATTYPE, FLOATTYPE, FLOATTYPE, FLOATTYPE); static FLOATNAME(LQuaternion) pure_imaginary(const FLOATNAME(LVector3) &); - INLINE_LINMATH FLOATNAME(LQuaternion) operator *(const FLOATNAME(LQuaternion) &); + INLINE_LINMATH FLOATNAME(LVecBase3) + xform(const FLOATNAME(LVecBase3) &v) const; + + INLINE_LINMATH FLOATNAME(LQuaternion) + multiply(const FLOATNAME(LQuaternion) &rhs) const; + + INLINE_LINMATH FLOATNAME(LQuaternion) operator - () const; + + INLINE_LINMATH FLOATNAME(LQuaternion) operator *(const FLOATNAME(LQuaternion) &) const; INLINE_LINMATH FLOATNAME(LQuaternion)& operator *=(const FLOATNAME(LQuaternion) &); INLINE_LINMATH FLOATNAME(LMatrix3) operator *(const FLOATNAME(LMatrix3) &);