From fc6db22bb173173244d3939faf547ab538068276 Mon Sep 17 00:00:00 2001 From: David Rose Date: Tue, 11 Dec 2001 18:50:55 +0000 Subject: [PATCH] rework quaternion logic for vrpn tracker --- panda/src/chan/animChannelMatrixXfmTable.cxx | 5 ++ panda/src/linmath/lquaternion_src.cxx | 78 ++++++++++---------- panda/src/mathutil/fftCompressor.cxx | 31 ++++++++ panda/src/mathutil/fftCompressor.h | 4 + panda/src/putil/bam.h | 3 +- 5 files changed, 81 insertions(+), 40 deletions(-) diff --git a/panda/src/chan/animChannelMatrixXfmTable.cxx b/panda/src/chan/animChannelMatrixXfmTable.cxx index f783e3ffa0..abfef0b10f 100644 --- a/panda/src/chan/animChannelMatrixXfmTable.cxx +++ b/panda/src/chan/animChannelMatrixXfmTable.cxx @@ -334,6 +334,11 @@ fillin(DatagramIterator& scan, BamReader* manager) } FFTCompressor compressor; + if (manager->get_file_minor_ver() < 4) { + // Prior to bam version 3.4, the quaternion code had been + // inadvertently transposed. + compressor.set_transpose_quats(true); + } compressor.read_header(scan); int i; diff --git a/panda/src/linmath/lquaternion_src.cxx b/panda/src/linmath/lquaternion_src.cxx index ef33abc11a..b0aa454312 100644 --- a/panda/src/linmath/lquaternion_src.cxx +++ b/panda/src/linmath/lquaternion_src.cxx @@ -32,14 +32,14 @@ pure_imaginary(const FLOATNAME(LVector3) &v) { } //////////////////////////////////////////////////////////////////// -// Function: extract (LMatrix3) -// Access: public -// Description: Do-While Jones paper from cary. +// Function: extract_to_matrix (LMatrix3) +// Access: Public +// Description: Based on the quat lib from VRPN. //////////////////////////////////////////////////////////////////// void FLOATNAME(LQuaternion):: extract_to_matrix(FLOATNAME(LMatrix3) &m) const { - 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 N = this->dot(*this); + FLOATTYPE s = (N == 0.0f) ? 0.0f : (2.0f / N); FLOATTYPE xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz; xs = _v.data[1] * s; ys = _v.data[2] * s; zs = _v.data[3] * s; @@ -47,20 +47,20 @@ extract_to_matrix(FLOATNAME(LMatrix3) &m) 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; - m.set((1. - (yy + zz)), (xy - wz), (xz + wy), - (xy + wz), (1. - (xx + zz)), (yz - wx), - (xz - wy), (yz + wx), (1. - (xx + yy))); + m.set((1.0f - (yy + zz)), (xy + wz), (xz - wy), + (xy - wz), (1.0f - (xx + zz)), (yz + wx), + (xz + wy), (yz - wx), (1.0f - (xx + yy))); } //////////////////////////////////////////////////////////////////// -// Function: extract (LMatrix4) -// Access: public -// Description: +// Function: extract_to_matrix (LMatrix4) +// Access: Public +// Description: Based on the quat lib from VRPN. //////////////////////////////////////////////////////////////////// void FLOATNAME(LQuaternion):: extract_to_matrix(FLOATNAME(LMatrix4) &m) const { - 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 N = this->dot(*this); + FLOATTYPE s = (N == 0.0f) ? 0.0f : (2.0f / N); FLOATTYPE xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz; xs = _v.data[1] * s; ys = _v.data[2] * s; zs = _v.data[3] * s; @@ -68,10 +68,10 @@ extract_to_matrix(FLOATNAME(LMatrix4) &m) 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; - m.set((1. - (yy + zz)), (xy - wz), (xz + wy), 0., - (xy + wz), (1. - (xx + zz)), (yz - wx), 0., - (xz - wy), (yz + wx), (1. - (xx + yy)), 0., - 0., 0., 0., 1.); + m.set((1.0f - (yy + zz)), (xy + wz), (xz - wy), 0.0f, + (xy - wz), (1.0f - (xx + zz)), (yz + wx), 0.0f, + (xz + wy), (yz - wx), (1.0f - (xx + yy)), 0.0f, + 0.0f, 0.0f, 0.0f, 1.0f); } //////////////////////////////////////////////////////////////////// @@ -161,37 +161,37 @@ get_hpr() const { // Description: Sets the quaternion according to the rotation // represented by the matrix. Originally we tried an // algorithm presented by Do-While Jones, but that -// turned out to be broken. +// turned out to be broken. This is based on the quat +// lib from UNC. //////////////////////////////////////////////////////////////////// void FLOATNAME(LQuaternion):: set_from_matrix(const FLOATNAME(LMatrix3) &m) { FLOATTYPE m00, m01, m02, m10, m11, m12, m20, m21, m22; m00 = m(0, 0); - m01 = m(0, 1); - m02 = m(0, 2); m10 = m(1, 0); - m11 = m(1, 1); - m12 = m(1, 2); m20 = m(2, 0); + m01 = m(0, 1); + m11 = m(1, 1); m21 = m(2, 1); + m02 = m(0, 2); + m12 = m(1, 2); m22 = m(2, 2); - FLOATTYPE T = m00 + m11 + m22; + FLOATTYPE trace = m00 + m11 + m22; - if (T > 0.0f) { + if (trace > 0.0f) { // The easy case. - FLOATTYPE S = csqrt(T + 1.0f); + FLOATTYPE S = csqrt(trace + 1.0f); _v.data[0] = S * 0.5f; S = 0.5f / S; - _v.data[1] = (m21 - m12) * S; - _v.data[2] = (m02 - m20) * S; - _v.data[3] = (m10 - m01) * S; + _v.data[1] = (m12 - m21) * S; + _v.data[2] = (m20 - m02) * S; + _v.data[3] = (m01 - m10) * S; } else { // The harder case. First, figure out which column to take as - // root. We'll choose the largest so that we get the greatest - // precision. + // root. This will be the column with the largest value. // It is tempting to try to compare the absolute values of the // diagonal values in the code below, instead of their normal, @@ -210,9 +210,9 @@ set_from_matrix(const FLOATNAME(LMatrix3) &m) { S = csqrt(S); _v.data[1] = S * 0.5f; S = 0.5f / S; - _v.data[2] = (m10 + m01) * S; - _v.data[3] = (m20 + m02) * S; - _v.data[0] = (m21 - m12) * S; + _v.data[2] = (m01 + m10) * S; + _v.data[3] = (m02 + m20) * S; + _v.data[0] = (m12 - m21) * S; } else if (m11 > m22) { // m11 is larger than m00 and m22. @@ -221,9 +221,9 @@ set_from_matrix(const FLOATNAME(LMatrix3) &m) { S = csqrt(S); _v.data[2] = S * 0.5f; S = 0.5f / S; - _v.data[3] = (m21 + m12) * S; - _v.data[1] = (m01 + m10) * S; - _v.data[0] = (m02 - m20) * S; + _v.data[3] = (m12 + m21) * S; + _v.data[1] = (m10 + m01) * S; + _v.data[0] = (m20 - m02) * S; } else { // m22 is larger than m00 and m11. @@ -232,9 +232,9 @@ set_from_matrix(const FLOATNAME(LMatrix3) &m) { S = csqrt(S); _v.data[3] = S * 0.5f; S = 0.5f / S; - _v.data[1] = (m02 + m20) * S; - _v.data[2] = (m12 + m21) * S; - _v.data[0] = (m10 - m01) * S; + _v.data[1] = (m20 + m02) * S; + _v.data[2] = (m21 + m12) * S; + _v.data[0] = (m01 - m10) * S; } } } diff --git a/panda/src/mathutil/fftCompressor.cxx b/panda/src/mathutil/fftCompressor.cxx index c7af938e0f..376c00337a 100644 --- a/panda/src/mathutil/fftCompressor.cxx +++ b/panda/src/mathutil/fftCompressor.cxx @@ -48,6 +48,7 @@ static RealPlans _real_decompress_plans; FFTCompressor:: FFTCompressor() { set_quality(-1); + _transpose_quats = false; } //////////////////////////////////////////////////////////////////// @@ -160,6 +161,30 @@ get_quality() const { return _quality; } +//////////////////////////////////////////////////////////////////// +// Function: FFTCompressor::set_transpose_quats +// Access: Public +// Description: Sets the transpose_quats flag. This is provided +// mainly for backward compatibility with old bam files +// that were written out with the quaternions +// inadvertently transposed. +//////////////////////////////////////////////////////////////////// +void FFTCompressor:: +set_transpose_quats(bool flag) { + _transpose_quats = flag; +} + +//////////////////////////////////////////////////////////////////// +// Function: FFTCompressor::get_transpose_quats +// Access: Public +// Description: Returns the transpose_quats flag. See +// set_transpose_quats(). +//////////////////////////////////////////////////////////////////// +bool FFTCompressor:: +get_transpose_quats() const { + return _transpose_quats; +} + //////////////////////////////////////////////////////////////////// // Function: FFTCompressor::write_header // Access: Public @@ -366,6 +391,9 @@ write_hprs(Datagram &datagram, const LVecBase3f *array, int length) { for (int i = 0; i < length; i++) { LMatrix3f mat; compose_matrix(mat, LVecBase3f(1.0, 1.0, 1.0), array[i]); + if (_transpose_quats) { + mat.transpose_in_place(); + } LOrientationf rot(mat); rot.normalize(); // This may not be necessary, but let's not take chances. @@ -663,6 +691,9 @@ read_hprs(DatagramIterator &di, vector_LVecBase3f &array) { LMatrix3f mat; rot.extract_to_matrix(mat); + if (_transpose_quats) { + mat.transpose_in_place(); + } LVecBase3f scale, hpr; bool success = decompose_matrix(mat, scale, hpr); nassertr(success, false); diff --git a/panda/src/mathutil/fftCompressor.h b/panda/src/mathutil/fftCompressor.h index 07afdb2445..d03a4ec7ef 100644 --- a/panda/src/mathutil/fftCompressor.h +++ b/panda/src/mathutil/fftCompressor.h @@ -54,6 +54,9 @@ public: void set_quality(int quality); int get_quality() const; + void set_transpose_quats(bool flag); + bool get_transpose_quats() const; + void write_header(Datagram &datagram); void write_reals(Datagram &datagram, const float *array, int length); void write_hprs(Datagram &datagram, const LVecBase3f *array, int length); @@ -90,6 +93,7 @@ private: double _fft_offset; double _fft_factor; double _fft_exponent; + bool _transpose_quats; }; #endif diff --git a/panda/src/putil/bam.h b/panda/src/putil/bam.h index 41b945b2fa..2c3a04f1e7 100644 --- a/panda/src/putil/bam.h +++ b/panda/src/putil/bam.h @@ -32,11 +32,12 @@ static const unsigned short _bam_major_ver = 3; // Bumped to major version 2 on 7/6/00 due to major changes in Character. // Bumped to major version 3 on 12/8/00 to change float64's to float32's. -static const unsigned short _bam_minor_ver = 3; +static const unsigned short _bam_minor_ver = 4; // Bumped to minor version 1 on 12/15/00 to add FFT-style channel // compression. // Bumped to minor version 2 on 2/15/01 to add ModelNode::_preserve_transform. // Bumped to minor version 3 on 4/11/01 to support correctly ordered children. +// Bumped to minor version 4 on 12/11/01 to transpose quaternions. #endif