diff --git a/panda/src/chan/animChannelMatrixXfmTable.cxx b/panda/src/chan/animChannelMatrixXfmTable.cxx index b332fdeb57..97c8505acd 100644 --- a/panda/src/chan/animChannelMatrixXfmTable.cxx +++ b/panda/src/chan/animChannelMatrixXfmTable.cxx @@ -28,6 +28,7 @@ #include "bamReader.h" #include "bamWriter.h" #include "fftCompressor.h" +#include "config_linmath.h" TypeHandle AnimChannelMatrixXfmTable::_type_handle; @@ -267,6 +268,7 @@ write_datagram(BamWriter *manager, Datagram &me) { FFTCompressor compressor; compressor.set_quality(compress_chan_quality); compressor.set_use_error_threshold(true); + me.add_bool(temp_hpr_fix); compressor.write_header(me); // First, write out the scales and shears. @@ -336,6 +338,14 @@ fillin(DatagramIterator &scan, BamReader *manager) { return; } + bool new_hpr = false; + if (manager->get_file_minor_ver() >= 14) { + // Beginning in bam 4.14, we encode a bool to indicate whether + // we used the old hpr or the new hpr calculation. Previously, + // we assume all bams used the old hpr calculation. + new_hpr = scan.get_bool(); + } + FFTCompressor compressor; compressor.read_header(scan, manager->get_file_minor_ver()); @@ -362,7 +372,7 @@ fillin(DatagramIterator &scan, BamReader *manager) { // Read in the HPR array and store it back in the joint angles. vector_LVecBase3f hprs; - compressor.read_hprs(scan, hprs); + compressor.read_hprs(scan, hprs, new_hpr); PTA_float h_table = PTA_float::empty_array(hprs.size()); PTA_float p_table = PTA_float::empty_array(hprs.size()); PTA_float r_table = PTA_float::empty_array(hprs.size()); diff --git a/panda/src/linmath/compose_matrix_src.I b/panda/src/linmath/compose_matrix_src.I index 01b3a06172..5bf50a4116 100644 --- a/panda/src/linmath/compose_matrix_src.I +++ b/panda/src/linmath/compose_matrix_src.I @@ -17,6 +17,24 @@ //////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////// +// Function: compose_matrix +// Description: Computes the 3x3 matrix from scale, shear, and +// rotation. +//////////////////////////////////////////////////////////////////// +INLINE_LINMATH void +compose_matrix(FLOATNAME(LMatrix3) &mat, + const FLOATNAME(LVecBase3) &scale, + const FLOATNAME(LVecBase3) &shear, + const FLOATNAME(LVecBase3) &hpr, + CoordinateSystem cs) { + if (temp_hpr_fix) { + compose_matrix_new_hpr(mat, scale, shear, hpr, cs); + } else { + compose_matrix_old_hpr(mat, scale, shear, hpr, cs); + } +} + //////////////////////////////////////////////////////////////////// // Function: compose_matrix // Description: Computes the 4x4 matrix according to scale, shear, @@ -29,30 +47,49 @@ compose_matrix(FLOATNAME(LMatrix4) &mat, const FLOATNAME(LVecBase3) &hpr, const FLOATNAME(LVecBase3) &translate, CoordinateSystem cs) { - FLOATNAME(LMatrix3) upper3; - compose_matrix(upper3, scale, shear, hpr, cs); - mat = FLOATNAME(LMatrix4)(upper3, translate); + if (temp_hpr_fix) { + compose_matrix_new_hpr(mat, scale, shear, hpr, translate, cs); + } else { + compose_matrix_old_hpr(mat, scale, shear, hpr, translate, cs); + } } +//////////////////////////////////////////////////////////////////// +// Function: compose_matrix +// Description: Computes the 4x4 matrix according to scale, shear, +// rotation, and translation. +//////////////////////////////////////////////////////////////////// INLINE_LINMATH void compose_matrix(FLOATNAME(LMatrix4) &mat, const FLOATTYPE components[num_matrix_components], CoordinateSystem cs) { - FLOATNAME(LVector3) scale(components[0], - components[1], - components[2]); - FLOATNAME(LVector3) shear(components[3], - components[4], - components[5]); - FLOATNAME(LVector3) hpr(components[6], - components[7], - components[8]); - FLOATNAME(LVector3) translate(components[9], - components[10], - components[11]); - compose_matrix(mat, scale, shear, hpr, translate, cs); + if (temp_hpr_fix) { + compose_matrix_new_hpr(mat, components, cs); + } else { + compose_matrix_old_hpr(mat, components, cs); + } } +//////////////////////////////////////////////////////////////////// +// Function: decompose_matrix +// Description: Extracts out the components of a 3x3 rotation matrix. +// Returns true if successful, or false if there was an +// error. Since a 3x3 matrix always contains an affine +// transform, this should succeed in the normal case; +// singular transforms are not treated as an error. +//////////////////////////////////////////////////////////////////// +INLINE_LINMATH bool +decompose_matrix(const FLOATNAME(LMatrix3) &mat, + FLOATNAME(LVecBase3) &scale, + FLOATNAME(LVecBase3) &shear, + FLOATNAME(LVecBase3) &hpr, + CoordinateSystem cs) { + if (temp_hpr_fix) { + return decompose_matrix_new_hpr(mat, scale, shear, hpr, cs); + } else { + return decompose_matrix_old_hpr(mat, scale, shear, hpr, cs); + } +} //////////////////////////////////////////////////////////////////// // Function: decompose_matrix @@ -68,9 +105,131 @@ decompose_matrix(const FLOATNAME(LMatrix4) &mat, FLOATNAME(LVecBase3) &hpr, FLOATNAME(LVecBase3) &translate, CoordinateSystem cs) { + if (temp_hpr_fix) { + return decompose_matrix_new_hpr(mat, scale, shear, hpr, translate, cs); + } else { + return decompose_matrix_old_hpr(mat, scale, shear, hpr, translate, cs); + } +} + +//////////////////////////////////////////////////////////////////// +// Function: decompose_matrix +// Description: Extracts out the components of an affine matrix. +// Returns true if the scale, shear, hpr, and translate +// completely describe the matrix, or false if the +// matrix is not affine. +//////////////////////////////////////////////////////////////////// +INLINE_LINMATH bool +decompose_matrix(const FLOATNAME(LMatrix4) &mat, + FLOATTYPE components[num_matrix_components], + CoordinateSystem cs) { + if (temp_hpr_fix) { + return decompose_matrix_new_hpr(mat, components, cs); + } else { + return decompose_matrix_old_hpr(mat, components, cs); + } +} + +// The following functions are deprecated; they have been replaced +// with new versions, above, that accept a shear component as well. + + +// Deprecated function. +INLINE EXPCL_PANDA void +compose_matrix(FLOATNAME(LMatrix3) &mat, + const FLOATNAME(LVecBase3) &scale, + const FLOATNAME(LVecBase3) &hpr, + CoordinateSystem cs) { + compose_matrix(mat, scale, FLOATNAME(LVecBase3)(0, 0, 0), hpr, cs); +} + +// Deprecated function. +INLINE_LINMATH void +compose_matrix(FLOATNAME(LMatrix4) &mat, + const FLOATNAME(LVecBase3) &scale, + const FLOATNAME(LVecBase3) &hpr, + const FLOATNAME(LVecBase3) &translate, + CoordinateSystem cs) { + FLOATNAME(LMatrix3) upper3; + compose_matrix(upper3, scale, hpr, cs); + mat = FLOATNAME(LMatrix4)(upper3, translate); +} + +// Deprecated function. +INLINE_LINMATH bool +decompose_matrix(const FLOATNAME(LMatrix3) &mat, + FLOATNAME(LVecBase3) &scale, + FLOATNAME(LVecBase3) &hpr, + CoordinateSystem cs) { + FLOATNAME(LVecBase3) shear; + if (!decompose_matrix(mat, scale, shear, hpr, cs)) { + return false; + } + return shear.almost_equal(FLOATNAME(LVecBase3)::zero()); +} + +// Deprecated function. +INLINE_LINMATH bool +decompose_matrix(const FLOATNAME(LMatrix4) &mat, + FLOATNAME(LVecBase3) &scale, + FLOATNAME(LVecBase3) &hpr, + FLOATNAME(LVecBase3) &translate, + CoordinateSystem cs) { // Get the translation first. mat.get_row3(translate,3); - if (!decompose_matrix(mat.get_upper_3(), scale, shear, hpr, cs)) { + return decompose_matrix(mat.get_upper_3(), scale, hpr, cs); +} + + +// The following functions are transitional and serve only to migrate +// code from the old, incorrect hpr calculations that Panda used to +// use. New code should not call these functions directly; use the +// unqualified functions, above, instead. + +// Transitional function. +INLINE_LINMATH void +compose_matrix_old_hpr(FLOATNAME(LMatrix4) &mat, + const FLOATNAME(LVecBase3) &scale, + const FLOATNAME(LVecBase3) &shear, + const FLOATNAME(LVecBase3) &hpr, + const FLOATNAME(LVecBase3) &translate, + CoordinateSystem cs) { + FLOATNAME(LMatrix3) upper3; + compose_matrix_old_hpr(upper3, scale, shear, hpr, cs); + mat = FLOATNAME(LMatrix4)(upper3, translate); +} + +// Transitional function. +INLINE_LINMATH void +compose_matrix_old_hpr(FLOATNAME(LMatrix4) &mat, + const FLOATTYPE components[num_matrix_components], + CoordinateSystem cs) { + FLOATNAME(LVector3) scale(components[0], + components[1], + components[2]); + FLOATNAME(LVector3) shear(components[3], + components[4], + components[5]); + FLOATNAME(LVector3) hpr(components[6], + components[7], + components[8]); + FLOATNAME(LVector3) translate(components[9], + components[10], + components[11]); + compose_matrix_old_hpr(mat, scale, shear, hpr, translate, cs); +} + +// Transitional function. +INLINE_LINMATH bool +decompose_matrix_old_hpr(const FLOATNAME(LMatrix4) &mat, + FLOATNAME(LVecBase3) &scale, + FLOATNAME(LVecBase3) &shear, + FLOATNAME(LVecBase3) &hpr, + FLOATNAME(LVecBase3) &translate, + CoordinateSystem cs) { + // Get the translation first. + mat.get_row3(translate,3); + if (!decompose_matrix_old_hpr(mat.get_upper_3(), scale, shear, hpr, cs)) { return false; } #ifndef NDEBUG @@ -80,12 +239,13 @@ decompose_matrix(const FLOATNAME(LMatrix4) &mat, #endif } +// Transitional function. INLINE_LINMATH bool -decompose_matrix(const FLOATNAME(LMatrix4) &mat, - FLOATTYPE components[num_matrix_components], - CoordinateSystem cs) { +decompose_matrix_old_hpr(const FLOATNAME(LMatrix4) &mat, + FLOATTYPE components[num_matrix_components], + CoordinateSystem cs) { FLOATNAME(LVector3) scale, shear, hpr, translate; - bool result = decompose_matrix(mat, scale, shear, hpr, translate, cs); + bool result = decompose_matrix_old_hpr(mat, scale, shear, hpr, translate, cs); components[0] = scale[0]; components[1] = scale[1]; components[2] = scale[2]; @@ -101,63 +261,77 @@ decompose_matrix(const FLOATNAME(LMatrix4) &mat, return result; } - - -// The following functions are deprecated; they have been replaced -// with new versions, above, that accept a shear component as well. - -INLINE EXPCL_PANDA void -compose_matrix(FLOATNAME(LMatrix3) &mat, - const FLOATNAME(LVecBase3) &scale, - const FLOATNAME(LVecBase3) &hpr, - CoordinateSystem cs) { - compose_matrix(mat, scale, FLOATNAME(LVecBase3)(0, 0, 0), hpr, cs); -} - -//////////////////////////////////////////////////////////////////// -// Function: compose_matrix -// Description: Computes the 4x4 matrix according to scale, rotation, -// and translation. -//////////////////////////////////////////////////////////////////// +// Transitional function. INLINE_LINMATH void -compose_matrix(FLOATNAME(LMatrix4) &mat, - const FLOATNAME(LVecBase3) &scale, - const FLOATNAME(LVecBase3) &hpr, - const FLOATNAME(LVecBase3) &translate, - CoordinateSystem cs) { +compose_matrix_new_hpr(FLOATNAME(LMatrix4) &mat, + const FLOATNAME(LVecBase3) &scale, + const FLOATNAME(LVecBase3) &shear, + const FLOATNAME(LVecBase3) &hpr, + const FLOATNAME(LVecBase3) &translate, + CoordinateSystem cs) { FLOATNAME(LMatrix3) upper3; - compose_matrix(upper3, scale, hpr, cs); + compose_matrix_new_hpr(upper3, scale, shear, hpr, cs); mat = FLOATNAME(LMatrix4)(upper3, translate); } -INLINE EXPCL_PANDA bool -decompose_matrix(const FLOATNAME(LMatrix3) &mat, - FLOATNAME(LVecBase3) &scale, - FLOATNAME(LVecBase3) &hpr, - CoordinateSystem cs) { - FLOATNAME(LVecBase3) shear; - if (!decompose_matrix(mat, scale, shear, hpr, cs)) { - return false; - } - return shear.almost_equal(FLOATNAME(LVecBase3)::zero()); +// Transitional function. +INLINE_LINMATH void +compose_matrix_new_hpr(FLOATNAME(LMatrix4) &mat, + const FLOATTYPE components[num_matrix_components], + CoordinateSystem cs) { + FLOATNAME(LVector3) scale(components[0], + components[1], + components[2]); + FLOATNAME(LVector3) shear(components[3], + components[4], + components[5]); + FLOATNAME(LVector3) hpr(components[6], + components[7], + components[8]); + FLOATNAME(LVector3) translate(components[9], + components[10], + components[11]); + compose_matrix_new_hpr(mat, scale, shear, hpr, translate, cs); } - -//////////////////////////////////////////////////////////////////// -// Function: decompose_matrix -// Description: Extracts out the components of an affine matrix. -// Returns true if the scale, hpr, translate -// completely describe the matrix, or false if there is -// also a shear component or if the matrix is not -// affine. -//////////////////////////////////////////////////////////////////// +// Transitional function. INLINE_LINMATH bool -decompose_matrix(const FLOATNAME(LMatrix4) &mat, - FLOATNAME(LVecBase3) &scale, - FLOATNAME(LVecBase3) &hpr, - FLOATNAME(LVecBase3) &translate, - CoordinateSystem cs) { +decompose_matrix_new_hpr(const FLOATNAME(LMatrix4) &mat, + FLOATNAME(LVecBase3) &scale, + FLOATNAME(LVecBase3) &shear, + FLOATNAME(LVecBase3) &hpr, + FLOATNAME(LVecBase3) &translate, + CoordinateSystem cs) { // Get the translation first. mat.get_row3(translate,3); - return decompose_matrix(mat.get_upper_3(), scale, hpr, cs); + if (!decompose_matrix_new_hpr(mat.get_upper_3(), scale, shear, hpr, cs)) { + return false; + } +#ifndef NDEBUG + return mat.get_col(3).almost_equal(FLOATNAME(LVecBase4)(0.0, 0.0, 0.0, 1.0)); +#else + return true; +#endif +} + +// Transitional function. +INLINE_LINMATH bool +decompose_matrix_new_hpr(const FLOATNAME(LMatrix4) &mat, + FLOATTYPE components[num_matrix_components], + CoordinateSystem cs) { + FLOATNAME(LVector3) scale, shear, hpr, translate; + bool result = decompose_matrix_new_hpr(mat, scale, shear, hpr, translate, cs); + components[0] = scale[0]; + components[1] = scale[1]; + components[2] = scale[2]; + components[3] = shear[0]; + components[4] = shear[1]; + components[5] = shear[2]; + components[6] = hpr[0]; + components[7] = hpr[1]; + components[8] = hpr[2]; + components[9] = translate[0]; + components[10] = translate[1]; + components[11] = translate[2]; + return result; } diff --git a/panda/src/linmath/compose_matrix_src.cxx b/panda/src/linmath/compose_matrix_src.cxx index 4926a6b1f5..4bbd44736f 100644 --- a/panda/src/linmath/compose_matrix_src.cxx +++ b/panda/src/linmath/compose_matrix_src.cxx @@ -18,38 +18,25 @@ //////////////////////////////////////////////////////////////////// -// Function: compose_matrix +// Function: compose_matrix_old_hpr // Description: Computes the 3x3 matrix from scale, shear, and // rotation. //////////////////////////////////////////////////////////////////// void -compose_matrix(FLOATNAME(LMatrix3) &mat, - const FLOATNAME(LVecBase3) &scale, - const FLOATNAME(LVecBase3) &shear, - const FLOATNAME(LVecBase3) &hpr, - CoordinateSystem cs) { - - // temp_hpr_fix blocks use the correct way. need to keep other way - // as default until legacy tools are fixed to work with correct way - - if (temp_hpr_fix) { - mat = - FLOATNAME(LMatrix3)::scale_shear_mat(scale, shear, cs) * - FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[2], FLOATNAME(LVector3)::forward(cs), cs) * - FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[1], FLOATNAME(LVector3)::right(cs), cs) * - FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[0], FLOATNAME(LVector3)::up(cs), cs); - - } else { - mat = - FLOATNAME(LMatrix3)::scale_shear_mat(scale, shear, cs) * - FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[1], FLOATNAME(LVector3)::right(cs), cs) * - FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[0], FLOATNAME(LVector3)::up(cs), cs) * - FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[2], FLOATNAME(LVector3)::back(cs), cs); - } +compose_matrix_old_hpr(FLOATNAME(LMatrix3) &mat, + const FLOATNAME(LVecBase3) &scale, + const FLOATNAME(LVecBase3) &shear, + const FLOATNAME(LVecBase3) &hpr, + CoordinateSystem cs) { + mat = + FLOATNAME(LMatrix3)::scale_shear_mat(scale, shear, cs) * + FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[1], FLOATNAME(LVector3)::right(cs), cs) * + FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[0], FLOATNAME(LVector3)::up(cs), cs) * + FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[2], FLOATNAME(LVector3)::back(cs), cs); } //////////////////////////////////////////////////////////////////// -// Function: unwind_yup_rotation +// Function: unwind_yup_rotation_old_hpr // Description: Extracts the rotation about the x, y, and z axes from // the given hpr & scale matrix. Adjusts the matrix // to eliminate the rotation. @@ -58,161 +45,7 @@ compose_matrix(FLOATNAME(LMatrix3) &mat, // right-handed Y-up coordinate system. //////////////////////////////////////////////////////////////////// static void -unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) { - - typedef FLOATNAME(LMatrix3) Matrix; - - if (temp_hpr_fix) { - // Extract the axes from the matrix. - FLOATNAME(LVector3) x, y, z; - mat.get_row(x,0); - mat.get_row(y,1); - mat.get_row(z,2); - - // Project Z into the XZ plane. - FLOATNAME(LVector2) xz(z[0], z[2]); - xz = normalize(xz); - - // Compute the rotation about the +Y (up) axis. This is yaw, or - // "heading". - FLOATTYPE heading = rad_2_deg(((FLOATTYPE)atan2(xz[0], xz[1]))); - - // Unwind the heading, and continue. - Matrix rot_y; - rot_y = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f), - CS_yup_right); - - x = x * rot_y; - y = y * rot_y; - z = z * rot_y; - - // Project the rotated Z into the YZ plane. - FLOATNAME(LVector2) yz(z[1], z[2]); - yz = normalize(yz); - - // Compute the rotation about the +X (right) axis. This is pitch. - FLOATTYPE pitch = rad_2_deg((FLOATTYPE)(-atan2(yz[0], yz[1]))); - - // Unwind the pitch. - Matrix rot_x; - rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f), - CS_yup_right); - - x = x * rot_x; - y = y * rot_x; - z = z * rot_x; - - // Project the rotated X onto the XY plane. - FLOATNAME(LVector2) xy(x[0], x[1]); - xy = normalize(xy); - - // Compute the rotation about the +Z (back) axis. This is roll. - FLOATTYPE roll = -rad_2_deg(((FLOATTYPE)atan2(xy[1], xy[0]))); - - // Unwind the roll from the axes, and continue. - Matrix rot_z; - rot_z = Matrix::rotate_mat_normaxis(roll, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f), - CS_yup_right); - - x = x * rot_z; - y = y * rot_z; - z = z * rot_z; - - // Reset the matrix to reflect the unwinding. - mat.set_row(0, x); - mat.set_row(1, y); - mat.set_row(2, z); - - // Return the three rotation components. - hpr[0] = heading; - hpr[1] = pitch; - hpr[2] = roll; - } else { - - // Extract the axes from the matrix. - FLOATNAME(LVector3) x, y, z; - mat.get_row(x,0); - mat.get_row(y,1); - mat.get_row(z,2); - - // Project X onto the XY plane. - FLOATNAME(LVector2) xy(x[0], x[1]); - xy = normalize(xy); - - // Compute the rotation about the +Z (back) axis. This is roll. - FLOATTYPE roll = rad_2_deg(((FLOATTYPE)atan2(xy[1], xy[0]))); - - // Unwind the roll from the axes, and continue. - Matrix rot_z; - rot_z = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f), - CS_yup_right); - - x = x * rot_z; - y = y * rot_z; - z = z * rot_z; - - // Project the rotated X into the XZ plane. - FLOATNAME(LVector2) xz(x[0], x[2]); - xz = normalize(xz); - - // Compute the rotation about the +Y (up) axis. This is yaw, or - // "heading". - FLOATTYPE heading = rad_2_deg(((FLOATTYPE)-atan2(xz[1], xz[0]))); - - // Unwind the heading, and continue. - Matrix rot_y; - rot_y = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f), - CS_yup_right); - - x = x * rot_y; - y = y * rot_y; - z = z * rot_y; - - // Project the rotated Z into the YZ plane. - FLOATNAME(LVector2) yz(z[1], z[2]); - yz = normalize(yz); - - // Compute the rotation about the +X (right) axis. This is pitch. - FLOATTYPE pitch = rad_2_deg(((FLOATTYPE)-atan2(yz[0], yz[1]))); - - // Unwind the pitch. - Matrix rot_x; - rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f), - CS_yup_right); - - x = x * rot_x; - y = y * rot_x; - z = z * rot_x; - - // Reset the matrix to reflect the unwinding. - mat.set_row(0, x); - mat.set_row(1, y); - mat.set_row(2, z); - - // Return the three rotation components. - hpr[0] = heading; - hpr[1] = pitch; - hpr[2] = roll; - } -} - -//////////////////////////////////////////////////////////////////// -// Function: unwind_yup_rotation -// Description: Extracts the rotation about the x, y, and z axes from -// the given hpr & scale matrix, given the indicated -// roll amount as a hint. Adjusts the matrix to -// eliminate the rotation. -// -// This function assumes the matrix is stored in a -// right-handed Y-up coordinate system. -//////////////////////////////////////////////////////////////////// -static void -unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr, - FLOATTYPE roll) { - if (temp_hpr_fix) { - unwind_yup_rotation(mat, hpr); - return; - } +unwind_yup_rotation_old_hpr(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) { typedef FLOATNAME(LMatrix3) Matrix; @@ -221,54 +54,61 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr, mat.get_row(x,0); mat.get_row(y,1); mat.get_row(z,2); - + + // Project X onto the XY plane. + FLOATNAME(LVector2) xy(x[0], x[1]); + xy = normalize(xy); + + // Compute the rotation about the +Z (back) axis. This is roll. + FLOATTYPE roll = rad_2_deg(((FLOATTYPE)atan2(xy[1], xy[0]))); + // Unwind the roll from the axes, and continue. Matrix rot_z; rot_z = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f), CS_yup_right); - + x = x * rot_z; y = y * rot_z; z = z * rot_z; - + // Project the rotated X into the XZ plane. FLOATNAME(LVector2) xz(x[0], x[2]); xz = normalize(xz); - + // Compute the rotation about the +Y (up) axis. This is yaw, or // "heading". FLOATTYPE heading = rad_2_deg(((FLOATTYPE)-atan2(xz[1], xz[0]))); - + // Unwind the heading, and continue. Matrix rot_y; rot_y = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f), CS_yup_right); - + x = x * rot_y; y = y * rot_y; z = z * rot_y; - + // Project the rotated Z into the YZ plane. FLOATNAME(LVector2) yz(z[1], z[2]); yz = normalize(yz); - + // Compute the rotation about the +X (right) axis. This is pitch. FLOATTYPE pitch = rad_2_deg(((FLOATTYPE)-atan2(yz[0], yz[1]))); - + // Unwind the pitch. Matrix rot_x; rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f), CS_yup_right); - + x = x * rot_x; y = y * rot_x; z = z * rot_x; - + // Reset the matrix to reflect the unwinding. mat.set_row(0, x); mat.set_row(1, y); mat.set_row(2, z); - + // Return the three rotation components. hpr[0] = heading; hpr[1] = pitch; @@ -276,7 +116,7 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr, } //////////////////////////////////////////////////////////////////// -// Function: unwind_zup_rotation +// Function: unwind_zup_rotation_old_hpr // Description: Extracts the rotation about the x, y, and z axes from // the given hpr & scale matrix. Adjusts the matrix // to eliminate the rotation. @@ -285,227 +125,78 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr, // right-handed Z-up coordinate system. //////////////////////////////////////////////////////////////////// static void -unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) { - if (temp_hpr_fix) { - typedef FLOATNAME(LMatrix3) Matrix; - - // Extract the axes from the matrix. - FLOATNAME(LVector3) x, y, z; - mat.get_row(x,0); - mat.get_row(y,1); - mat.get_row(z,2); - - // Project Y into the XY plane. - FLOATNAME(LVector2) xy(y[0], y[1]); - xy = normalize(xy); - - // Compute the rotation about the +Z (up) axis. This is yaw, or - // "heading". - FLOATTYPE heading = -rad_2_deg(((FLOATTYPE)atan2(xy[0], xy[1]))); - - // Unwind the heading, and continue. - Matrix rot_z; - rot_z = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f), - CS_zup_right); - - x = x * rot_z; - y = y * rot_z; - z = z * rot_z; - - // Project the rotated Y into the YZ plane. - FLOATNAME(LVector2) yz(y[1], y[2]); - yz = normalize(yz); - - // Compute the rotation about the +X (right) axis. This is pitch. - FLOATTYPE pitch = rad_2_deg(((FLOATTYPE)atan2(yz[1], yz[0]))); - - // Unwind the pitch. - Matrix rot_x; - rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f), - CS_zup_right); - - x = x * rot_x; - y = y * rot_x; - z = z * rot_x; - - // Project X into the XZ plane. - FLOATNAME(LVector2) xz(x[0], x[2]); - xz = normalize(xz); - - // Compute the rotation about the -Y (back) axis. This is roll. - FLOATTYPE roll = -rad_2_deg(((FLOATTYPE)atan2(xz[1], xz[0]))); - - // Unwind the roll from the axes, and continue. - Matrix rot_y; - rot_y = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f), - CS_zup_right); - - x = x * rot_y; - y = y * rot_y; - z = z * rot_y; - - // Reset the matrix to reflect the unwinding. - mat.set_row(0, x); - mat.set_row(1, y); - mat.set_row(2, z); - - // Return the three rotation components. - hpr[0] = heading; - hpr[1] = pitch; - hpr[2] = roll; - - } else { - typedef FLOATNAME(LMatrix3) Matrix; - - // Extract the axes from the matrix. - FLOATNAME(LVector3) x, y, z; - mat.get_row(x,0); - mat.get_row(y,1); - mat.get_row(z,2); - - - // Project X into the XZ plane. - FLOATNAME(LVector2) xz(x[0], x[2]); - xz = normalize(xz); - - // Compute the rotation about the -Y (back) axis. This is roll. - FLOATTYPE roll = rad_2_deg(((FLOATTYPE)atan2(xz[1], xz[0]))); - - if (y[1] < 0.0f) { - if (roll < 0.0f) { - roll += 180.0; - } else { - roll -= 180.0; - } - } - - // Unwind the roll from the axes, and continue. - Matrix rot_y; - rot_y = Matrix::rotate_mat_normaxis(roll, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f), - CS_zup_right); - - x = x * rot_y; - y = y * rot_y; - z = z * rot_y; - - // Project the rotated X into the XY plane. - FLOATNAME(LVector2) xy(x[0], x[1]); - xy = normalize(xy); - - // Compute the rotation about the +Z (up) axis. This is yaw, or - // "heading". - FLOATTYPE heading = rad_2_deg(((FLOATTYPE)atan2(xy[1], xy[0]))); - - // Unwind the heading, and continue. - Matrix rot_z; - rot_z = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f), - CS_zup_right); - - x = x * rot_z; - y = y * rot_z; - z = z * rot_z; - - // Project the rotated Y into the YZ plane. - FLOATNAME(LVector2) yz(y[1], y[2]); - yz = normalize(yz); - - // Compute the rotation about the +X (right) axis. This is pitch. - FLOATTYPE pitch = rad_2_deg(((FLOATTYPE)atan2(yz[1], yz[0]))); - - // Unwind the pitch. - Matrix rot_x; - rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f), - CS_zup_right); - - x = x * rot_x; - y = y * rot_x; - z = z * rot_x; - - // Reset the matrix to reflect the unwinding. - mat.set_row(0, x); - mat.set_row(1, y); - mat.set_row(2, z); - - // Return the three rotation components. - hpr[0] = heading; - hpr[1] = pitch; - hpr[2] = roll; - } -} - -//////////////////////////////////////////////////////////////////// -// Function: unwind_zup_rotation -// Description: Extracts the rotation about the x, y, and z axes from -// the given hpr & scale matrix, given the indicated -// roll amount as a hint. Adjusts the matrix to -// eliminate the rotation. -// -// This function assumes the matrix is stored in a -// right-handed Z-up coordinate system. -//////////////////////////////////////////////////////////////////// -static void -unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr, - FLOATTYPE roll) { - if (temp_hpr_fix) { - unwind_zup_rotation(mat, hpr); - return; - } - +unwind_zup_rotation_old_hpr(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) { typedef FLOATNAME(LMatrix3) Matrix; - + // Extract the axes from the matrix. FLOATNAME(LVector3) x, y, z; mat.get_row(x,0); mat.get_row(y,1); mat.get_row(z,2); - + + + // Project X into the XZ plane. + FLOATNAME(LVector2) xz(x[0], x[2]); + xz = normalize(xz); + + // Compute the rotation about the -Y (back) axis. This is roll. + FLOATTYPE roll = rad_2_deg(((FLOATTYPE)atan2(xz[1], xz[0]))); + + if (y[1] < 0.0f) { + if (roll < 0.0f) { + roll += 180.0; + } else { + roll -= 180.0; + } + } + // Unwind the roll from the axes, and continue. Matrix rot_y; rot_y = Matrix::rotate_mat_normaxis(roll, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f), CS_zup_right); - + x = x * rot_y; y = y * rot_y; z = z * rot_y; - + // Project the rotated X into the XY plane. FLOATNAME(LVector2) xy(x[0], x[1]); xy = normalize(xy); - + // Compute the rotation about the +Z (up) axis. This is yaw, or // "heading". FLOATTYPE heading = rad_2_deg(((FLOATTYPE)atan2(xy[1], xy[0]))); - + // Unwind the heading, and continue. Matrix rot_z; rot_z = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f), CS_zup_right); - + x = x * rot_z; y = y * rot_z; z = z * rot_z; - + // Project the rotated Y into the YZ plane. FLOATNAME(LVector2) yz(y[1], y[2]); yz = normalize(yz); - + // Compute the rotation about the +X (right) axis. This is pitch. FLOATTYPE pitch = rad_2_deg(((FLOATTYPE)atan2(yz[1], yz[0]))); - + // Unwind the pitch. Matrix rot_x; rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f), CS_zup_right); - + x = x * rot_x; y = y * rot_x; z = z * rot_x; - + // Reset the matrix to reflect the unwinding. mat.set_row(0, x); mat.set_row(1, y); mat.set_row(2, z); - + // Return the three rotation components. hpr[0] = heading; hpr[1] = pitch; @@ -513,7 +204,7 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr, } //////////////////////////////////////////////////////////////////// -// Function: decompose_matrix +// Function: decompose_matrix_old_hpr // Description: Extracts out the components of a 3x3 rotation matrix. // Returns true if successful, or false if there was an // error. Since a 3x3 matrix always contains an affine @@ -521,11 +212,11 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr, // singular transforms are not treated as an error. //////////////////////////////////////////////////////////////////// bool -decompose_matrix(const FLOATNAME(LMatrix3) &mat, - FLOATNAME(LVecBase3) &scale, - FLOATNAME(LVecBase3) &shear, - FLOATNAME(LVecBase3) &hpr, - CoordinateSystem cs) { +decompose_matrix_old_hpr(const FLOATNAME(LMatrix3) &mat, + FLOATNAME(LVecBase3) &scale, + FLOATNAME(LVecBase3) &shear, + FLOATNAME(LVecBase3) &hpr, + CoordinateSystem cs) { if (cs == CS_default) { cs = get_default_coordinate_system(); } @@ -544,14 +235,14 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat, switch (cs) { case CS_zup_right: { - unwind_zup_rotation(new_mat, hpr); + unwind_zup_rotation_old_hpr(new_mat, hpr); is_left_handed = false; } break; case CS_yup_right: { - unwind_yup_rotation(new_mat, hpr); + unwind_yup_rotation_old_hpr(new_mat, hpr); is_left_handed = false; } break; @@ -567,7 +258,7 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat, mat(1, 0), mat(1, 1), -mat(1, 2), -mat(2, 0), -mat(2, 1), mat(2, 2)); */ - unwind_zup_rotation(new_mat, hpr); + unwind_zup_rotation_old_hpr(new_mat, hpr); hpr[0] = -hpr[0]; hpr[2] = -hpr[2]; is_left_handed = true; @@ -585,7 +276,7 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat, mat(1, 0), mat(1, 1), -mat(1, 2), -mat(2, 0), -mat(2, 1), mat(2, 2)); */ - unwind_yup_rotation(new_mat, hpr); + unwind_yup_rotation_old_hpr(new_mat, hpr); is_left_handed = true; } break; @@ -625,37 +316,205 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat, return true; } + //////////////////////////////////////////////////////////////////// -// Function: decompose_matrix +// Function: compose_matrix_new_hpr +// Description: Computes the 3x3 matrix from scale, shear, and +// rotation. +//////////////////////////////////////////////////////////////////// +void +compose_matrix_new_hpr(FLOATNAME(LMatrix3) &mat, + const FLOATNAME(LVecBase3) &scale, + const FLOATNAME(LVecBase3) &shear, + const FLOATNAME(LVecBase3) &hpr, + CoordinateSystem cs) { + mat = + FLOATNAME(LMatrix3)::scale_shear_mat(scale, shear, cs) * + FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[2], FLOATNAME(LVector3)::forward(cs), cs) * + FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[1], FLOATNAME(LVector3)::right(cs), cs) * + FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[0], FLOATNAME(LVector3)::up(cs), cs); +} + +//////////////////////////////////////////////////////////////////// +// Function: unwind_yup_rotation_new_hpr +// Description: Extracts the rotation about the x, y, and z axes from +// the given hpr & scale matrix. Adjusts the matrix +// to eliminate the rotation. +// +// This function assumes the matrix is stored in a +// right-handed Y-up coordinate system. +//////////////////////////////////////////////////////////////////// +static void +unwind_yup_rotation_new_hpr(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) { + + typedef FLOATNAME(LMatrix3) Matrix; + + // Extract the axes from the matrix. + FLOATNAME(LVector3) x, y, z; + mat.get_row(x,0); + mat.get_row(y,1); + mat.get_row(z,2); + + // Project Z into the XZ plane. + FLOATNAME(LVector2) xz(z[0], z[2]); + xz = normalize(xz); + + // Compute the rotation about the +Y (up) axis. This is yaw, or + // "heading". + FLOATTYPE heading = rad_2_deg(((FLOATTYPE)atan2(xz[0], xz[1]))); + + // Unwind the heading, and continue. + Matrix rot_y; + rot_y = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f), + CS_yup_right); + + x = x * rot_y; + y = y * rot_y; + z = z * rot_y; + + // Project the rotated Z into the YZ plane. + FLOATNAME(LVector2) yz(z[1], z[2]); + yz = normalize(yz); + + // Compute the rotation about the +X (right) axis. This is pitch. + FLOATTYPE pitch = rad_2_deg((FLOATTYPE)(-atan2(yz[0], yz[1]))); + + // Unwind the pitch. + Matrix rot_x; + rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f), + CS_yup_right); + + x = x * rot_x; + y = y * rot_x; + z = z * rot_x; + + // Project the rotated X onto the XY plane. + FLOATNAME(LVector2) xy(x[0], x[1]); + xy = normalize(xy); + + // Compute the rotation about the +Z (back) axis. This is roll. + FLOATTYPE roll = -rad_2_deg(((FLOATTYPE)atan2(xy[1], xy[0]))); + + // Unwind the roll from the axes, and continue. + Matrix rot_z; + rot_z = Matrix::rotate_mat_normaxis(roll, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f), + CS_yup_right); + + x = x * rot_z; + y = y * rot_z; + z = z * rot_z; + + // Reset the matrix to reflect the unwinding. + mat.set_row(0, x); + mat.set_row(1, y); + mat.set_row(2, z); + + // Return the three rotation components. + hpr[0] = heading; + hpr[1] = pitch; + hpr[2] = roll; +} + +//////////////////////////////////////////////////////////////////// +// Function: unwind_zup_rotation_new_hpr +// Description: Extracts the rotation about the x, y, and z axes from +// the given hpr & scale matrix. Adjusts the matrix +// to eliminate the rotation. +// +// This function assumes the matrix is stored in a +// right-handed Z-up coordinate system. +//////////////////////////////////////////////////////////////////// +static void +unwind_zup_rotation_new_hpr(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) { + typedef FLOATNAME(LMatrix3) Matrix; + + // Extract the axes from the matrix. + FLOATNAME(LVector3) x, y, z; + mat.get_row(x,0); + mat.get_row(y,1); + mat.get_row(z,2); + + // Project Y into the XY plane. + FLOATNAME(LVector2) xy(y[0], y[1]); + xy = normalize(xy); + + // Compute the rotation about the +Z (up) axis. This is yaw, or + // "heading". + FLOATTYPE heading = -rad_2_deg(((FLOATTYPE)atan2(xy[0], xy[1]))); + + // Unwind the heading, and continue. + Matrix rot_z; + rot_z = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f), + CS_zup_right); + + x = x * rot_z; + y = y * rot_z; + z = z * rot_z; + + // Project the rotated Y into the YZ plane. + FLOATNAME(LVector2) yz(y[1], y[2]); + yz = normalize(yz); + + // Compute the rotation about the +X (right) axis. This is pitch. + FLOATTYPE pitch = rad_2_deg(((FLOATTYPE)atan2(yz[1], yz[0]))); + + // Unwind the pitch. + Matrix rot_x; + rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f), + CS_zup_right); + + x = x * rot_x; + y = y * rot_x; + z = z * rot_x; + + // Project X into the XZ plane. + FLOATNAME(LVector2) xz(x[0], x[2]); + xz = normalize(xz); + + // Compute the rotation about the -Y (back) axis. This is roll. + FLOATTYPE roll = -rad_2_deg(((FLOATTYPE)atan2(xz[1], xz[0]))); + + // Unwind the roll from the axes, and continue. + Matrix rot_y; + rot_y = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f), + CS_zup_right); + + x = x * rot_y; + y = y * rot_y; + z = z * rot_y; + + // Reset the matrix to reflect the unwinding. + mat.set_row(0, x); + mat.set_row(1, y); + mat.set_row(2, z); + + // Return the three rotation components. + hpr[0] = heading; + hpr[1] = pitch; + hpr[2] = roll; +} + +//////////////////////////////////////////////////////////////////// +// Function: decompose_matrix_new_hpr // Description: Extracts out the components of a 3x3 rotation matrix. -// Returns true if the scale and hpr completely describe -// the matrix, or false if there is also a shear -// component or if the matrix is not affine. -// -// This flavor of the function accepts an expected roll -// amount. This amount will be used as the roll -// component, rather than attempting to determine roll -// by examining the matrix; this helps alleviate roll -// instability due to roundoff errors or gimbal lock. -// -// This function is deprecated and will soon be removed, -// especially when the need for temp_hpr_fix is -// eliminated. +// Returns true if successful, or false if there was an +// error. Since a 3x3 matrix always contains an affine +// transform, this should succeed in the normal case; +// singular transforms are not treated as an error. //////////////////////////////////////////////////////////////////// bool -decompose_matrix(const FLOATNAME(LMatrix3) &mat, - FLOATNAME(LVecBase3) &scale, - FLOATNAME(LVecBase3) &hpr, - FLOATTYPE roll, - CoordinateSystem cs) { +decompose_matrix_new_hpr(const FLOATNAME(LMatrix3) &mat, + FLOATNAME(LVecBase3) &scale, + FLOATNAME(LVecBase3) &shear, + FLOATNAME(LVecBase3) &hpr, + CoordinateSystem cs) { if (cs == CS_default) { cs = get_default_coordinate_system(); } if (linmath_cat.is_debug()) { linmath_cat.debug() - << "decomposing " << mat << " via cs " << cs - << " with roll = " << roll << "\n"; + << "decomposing " << mat << " via cs " << cs << "\n"; } // Extract the rotation and scale, according to the coordinate @@ -667,14 +526,14 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat, switch (cs) { case CS_zup_right: { - unwind_zup_rotation(new_mat, hpr, roll); + unwind_zup_rotation_new_hpr(new_mat, hpr); is_left_handed = false; } break; case CS_yup_right: { - unwind_yup_rotation(new_mat, hpr, roll); + unwind_yup_rotation_new_hpr(new_mat, hpr); is_left_handed = false; } break; @@ -690,7 +549,9 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat, mat(1, 0), mat(1, 1), -mat(1, 2), -mat(2, 0), -mat(2, 1), mat(2, 2)); */ - unwind_zup_rotation(new_mat, hpr, roll); + unwind_zup_rotation_new_hpr(new_mat, hpr); + hpr[0] = -hpr[0]; + hpr[2] = -hpr[2]; is_left_handed = true; } break; @@ -706,7 +567,7 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat, mat(1, 0), mat(1, 1), -mat(1, 2), -mat(2, 0), -mat(2, 1), mat(2, 2)); */ - unwind_yup_rotation(new_mat, hpr, roll); + unwind_yup_rotation_new_hpr(new_mat, hpr); is_left_handed = true; } break; @@ -722,21 +583,53 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat, << "after unwind, mat is " << new_mat << "\n"; } - scale[0] = new_mat._m.m._00; - scale[1] = new_mat._m.m._11; - scale[2] = new_mat._m.m._22; - /* - if (is_left_handed) { - scale[0] = -new_mat._m.m._00; - scale[1] = -new_mat._m.m._11; + scale.set(new_mat(0, 0), new_mat(1, 1), new_mat(2, 2)); + + // Normalize the scale out of the shear components, and return the + // shear. + if (scale[0] != 0.0) { + new_mat(0, 1) /= scale[0]; + new_mat(0, 2) /= scale[0]; + } + if (scale[1] != 0.0) { + new_mat(1, 0) /= scale[1]; + new_mat(1, 2) /= scale[1]; + } + if (scale[2] != 0.0) { + new_mat(2, 0) /= scale[2]; + new_mat(2, 1) /= scale[2]; } - */ - bool has_no_shear = - (fabs(new_mat(0, 1)) + fabs(new_mat(0, 2)) + - fabs(new_mat(1, 0)) + fabs(new_mat(1, 2)) + - fabs(new_mat(2, 0)) + fabs(new_mat(2, 1))) < 0.0001; - - return has_no_shear; + shear.set(new_mat(0, 1) + new_mat(1, 0), + new_mat(2, 0) + new_mat(0, 2), + new_mat(2, 1) + new_mat(1, 2)); + + return true; } + +//////////////////////////////////////////////////////////////////// +// Function: old_to_new_hpr +// Description: Converts the HPR as represented in the old, broken +// way to the new, correct representation. Returns the +// new HPR. +// +// This function is provided to ease transition from old +// systems that relied on Panda's original broken HPR +// calculation. +//////////////////////////////////////////////////////////////////// +FLOATNAME(LVecBase3) +old_to_new_hpr(const FLOATNAME(LVecBase3) &old_hpr) { + FLOATNAME(LMatrix3) mat; + compose_matrix_old_hpr(mat, + FLOATNAME(LVecBase3)(1.0f, 1.0f, 1.0f), + FLOATNAME(LVecBase3)::zero(), + old_hpr); + + FLOATNAME(LVecBase3) new_scale; + FLOATNAME(LVecBase3) new_shear; + FLOATNAME(LVecBase3) new_hpr; + + decompose_matrix_new_hpr(mat, new_scale, new_shear, new_hpr); + return new_hpr; +} diff --git a/panda/src/linmath/compose_matrix_src.h b/panda/src/linmath/compose_matrix_src.h index dc17eaeac4..fb204e4daf 100644 --- a/panda/src/linmath/compose_matrix_src.h +++ b/panda/src/linmath/compose_matrix_src.h @@ -18,7 +18,7 @@ BEGIN_PUBLISH -EXPCL_PANDA void +INLINE_LINMATH void compose_matrix(FLOATNAME(LMatrix3) &mat, const FLOATNAME(LVecBase3) &scale, const FLOATNAME(LVecBase3) &shear, @@ -34,10 +34,11 @@ compose_matrix(FLOATNAME(LMatrix4) &mat, CoordinateSystem cs = CS_default); INLINE_LINMATH void -compose_matrix(FLOATNAME(LMatrix4) &mat, const FLOATTYPE components[num_matrix_components], +compose_matrix(FLOATNAME(LMatrix4) &mat, + const FLOATTYPE components[num_matrix_components], CoordinateSystem cs = CS_default); -EXPCL_PANDA bool +INLINE_LINMATH bool decompose_matrix(const FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &scale, FLOATNAME(LVecBase3) &shear, @@ -61,7 +62,7 @@ decompose_matrix(const FLOATNAME(LMatrix4) &mat, FLOATTYPE components[num_matrix // The following functions are deprecated; they have been replaced // with new versions, above, that accept a shear component as well. -INLINE EXPCL_PANDA void +INLINE_LINMATH void compose_matrix(FLOATNAME(LMatrix3) &mat, const FLOATNAME(LVecBase3) &scale, const FLOATNAME(LVecBase3) &hpr, @@ -74,19 +75,12 @@ compose_matrix(FLOATNAME(LMatrix4) &mat, const FLOATNAME(LVecBase3) &translate, CoordinateSystem cs = CS_default); -INLINE EXPCL_PANDA bool +INLINE_LINMATH bool decompose_matrix(const FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &scale, FLOATNAME(LVecBase3) &hpr, CoordinateSystem cs = CS_default); -EXPCL_PANDA bool -decompose_matrix(const FLOATNAME(LMatrix3) &mat, - FLOATNAME(LVecBase3) &scale, - FLOATNAME(LVecBase3) &hpr, - FLOATTYPE roll, - CoordinateSystem cs); - INLINE_LINMATH bool decompose_matrix(const FLOATNAME(LMatrix4) &mat, FLOATNAME(LVecBase3) &scale, @@ -94,6 +88,93 @@ decompose_matrix(const FLOATNAME(LMatrix4) &mat, FLOATNAME(LVecBase3) &translate, CoordinateSystem cs = CS_default); + +// The following functions are transitional and serve only to migrate +// code from the old, incorrect hpr calculations that Panda used to +// use. New code should not call these functions directly; use the +// unqualified functions, above, instead. + +EXPCL_PANDA void +compose_matrix_old_hpr(FLOATNAME(LMatrix3) &mat, + const FLOATNAME(LVecBase3) &scale, + const FLOATNAME(LVecBase3) &shear, + const FLOATNAME(LVecBase3) &hpr, + CoordinateSystem cs = CS_default); + +INLINE_LINMATH void +compose_matrix_old_hpr(FLOATNAME(LMatrix4) &mat, + const FLOATNAME(LVecBase3) &scale, + const FLOATNAME(LVecBase3) &shear, + const FLOATNAME(LVecBase3) &hpr, + const FLOATNAME(LVecBase3) &translate, + CoordinateSystem cs = CS_default); + +INLINE_LINMATH void +compose_matrix_old_hpr(FLOATNAME(LMatrix4) &mat, const FLOATTYPE components[num_matrix_components], + CoordinateSystem cs = CS_default); + +EXPCL_PANDA bool +decompose_matrix_old_hpr(const FLOATNAME(LMatrix3) &mat, + FLOATNAME(LVecBase3) &scale, + FLOATNAME(LVecBase3) &shear, + FLOATNAME(LVecBase3) &hpr, + CoordinateSystem cs = CS_default); + +INLINE_LINMATH bool +decompose_matrix_old_hpr(const FLOATNAME(LMatrix4) &mat, + FLOATNAME(LVecBase3) &scale, + FLOATNAME(LVecBase3) &shear, + FLOATNAME(LVecBase3) &hpr, + FLOATNAME(LVecBase3) &translate, + CoordinateSystem cs = CS_default); + +INLINE_LINMATH bool +decompose_matrix_old_hpr(const FLOATNAME(LMatrix4) &mat, FLOATTYPE components[num_matrix_components], + CoordinateSystem CS = CS_default); + + +EXPCL_PANDA void +compose_matrix_new_hpr(FLOATNAME(LMatrix3) &mat, + const FLOATNAME(LVecBase3) &scale, + const FLOATNAME(LVecBase3) &shear, + const FLOATNAME(LVecBase3) &hpr, + CoordinateSystem cs = CS_default); + +INLINE_LINMATH void +compose_matrix_new_hpr(FLOATNAME(LMatrix4) &mat, + const FLOATNAME(LVecBase3) &scale, + const FLOATNAME(LVecBase3) &shear, + const FLOATNAME(LVecBase3) &hpr, + const FLOATNAME(LVecBase3) &translate, + CoordinateSystem cs = CS_default); + +INLINE_LINMATH void +compose_matrix_new_hpr(FLOATNAME(LMatrix4) &mat, const FLOATTYPE components[num_matrix_components], + CoordinateSystem cs = CS_default); + +EXPCL_PANDA bool +decompose_matrix_new_hpr(const FLOATNAME(LMatrix3) &mat, + FLOATNAME(LVecBase3) &scale, + FLOATNAME(LVecBase3) &shear, + FLOATNAME(LVecBase3) &hpr, + CoordinateSystem cs = CS_default); + +INLINE_LINMATH bool +decompose_matrix_new_hpr(const FLOATNAME(LMatrix4) &mat, + FLOATNAME(LVecBase3) &scale, + FLOATNAME(LVecBase3) &shear, + FLOATNAME(LVecBase3) &hpr, + FLOATNAME(LVecBase3) &translate, + CoordinateSystem cs = CS_default); + +INLINE_LINMATH bool +decompose_matrix_new_hpr(const FLOATNAME(LMatrix4) &mat, FLOATTYPE components[num_matrix_components], + CoordinateSystem CS = CS_default); + + +EXPCL_PANDA FLOATNAME(LVecBase3) +old_to_new_hpr(const FLOATNAME(LVecBase3) &old_hpr); + END_PUBLISH #include "compose_matrix_src.I" diff --git a/panda/src/mathutil/fftCompressor.cxx b/panda/src/mathutil/fftCompressor.cxx index 44da283e9f..23f56ce6fd 100644 --- a/panda/src/mathutil/fftCompressor.cxx +++ b/panda/src/mathutil/fftCompressor.cxx @@ -18,6 +18,7 @@ #include "fftCompressor.h" #include "config_mathutil.h" +#include "config_linmath.h" #include "datagram.h" #include "datagramIterator.h" @@ -646,9 +647,15 @@ read_reals(DatagramIterator &di, vector_float &array) { // onto the end of the indicated vector, which is not // cleared first; it is the user's responsibility to // ensure that the array is initially empty. +// +// new_hpr is a temporary, transitional parameter. If +// it is set false, the hprs are decompressed according +// to the old, broken hpr calculation; if true, the hprs +// are decompressed according to the new, correct hpr +// calculation. See temp_hpr_fix. //////////////////////////////////////////////////////////////////// bool FFTCompressor:: -read_hprs(DatagramIterator &di, vector_LVecBase3f &array) { +read_hprs(DatagramIterator &di, vector_LVecBase3f &array, bool new_hpr) { #ifndef NDEBUG if (_quality >= 104) { // If quality level is at least 104, we don't even convert hpr to @@ -663,7 +670,7 @@ read_hprs(DatagramIterator &di, vector_LVecBase3f &array) { if (okflag) { nassertr(h.size() == p.size() && p.size() == r.size(), false); for (int i = 0; i < (int)h.size(); i++) { - array.push_back(LVecBase3f(h[i], p[i], r[i])); + array.push_back(LVecBase3f(h[i], p[i], r[i])); } } @@ -694,7 +701,11 @@ read_hprs(DatagramIterator &di, vector_LVecBase3f &array) { m10[i], m11[i], m12[i], m20[i], m21[i], m22[i]); LVecBase3f scale, shear, hpr; - decompose_matrix(mat, scale, shear, hpr); + if (new_hpr) { + decompose_matrix_new_hpr(mat, scale, shear, hpr); + } else { + decompose_matrix_old_hpr(mat, scale, shear, hpr); + } array.push_back(hpr); } } @@ -753,7 +764,11 @@ read_hprs(DatagramIterator &di, vector_LVecBase3f &array) { mat.transpose_in_place(); } LVecBase3f scale, shear, hpr; - decompose_matrix(mat, scale, shear, hpr); + if (new_hpr) { + decompose_matrix_new_hpr(mat, scale, shear, hpr); + } else { + decompose_matrix_old_hpr(mat, scale, shear, hpr); + } array.push_back(hpr); } } @@ -761,6 +776,19 @@ read_hprs(DatagramIterator &di, vector_LVecBase3f &array) { return okflag; } +//////////////////////////////////////////////////////////////////// +// Function: FFTCompressor::read_hprs +// Access: Public +// Description: Reads an array of HPR angles. The result is pushed +// onto the end of the indicated vector, which is not +// cleared first; it is the user's responsibility to +// ensure that the array is initially empty. +//////////////////////////////////////////////////////////////////// +bool FFTCompressor:: +read_hprs(DatagramIterator &di, vector_LVecBase3f &array) { + return read_hprs(di, array, temp_hpr_fix); +} + //////////////////////////////////////////////////////////////////// // Function: FFTCompressor::free_storage diff --git a/panda/src/mathutil/fftCompressor.h b/panda/src/mathutil/fftCompressor.h index 9679b0e9de..e05081481e 100644 --- a/panda/src/mathutil/fftCompressor.h +++ b/panda/src/mathutil/fftCompressor.h @@ -66,6 +66,8 @@ public: bool read_header(DatagramIterator &di, int bam_minor_version); bool read_reals(DatagramIterator &di, vector_float &array); + bool read_hprs(DatagramIterator &di, vector_LVecBase3f &array, + bool new_hpr); bool read_hprs(DatagramIterator &di, vector_LVecBase3f &array); static void free_storage(); diff --git a/panda/src/putil/bam.h b/panda/src/putil/bam.h index 6abfbceecc..f705c440fc 100644 --- a/panda/src/putil/bam.h +++ b/panda/src/putil/bam.h @@ -34,7 +34,7 @@ static const unsigned short _bam_major_ver = 4; // Bumped to major version 3 on 12/8/00 to change float64's to float32's. // Bumped to major version 4 on 4/10/02 to store new scene graph. -static const unsigned short _bam_minor_ver = 13; +static const unsigned short _bam_minor_ver = 14; // Bumped to minor version 1 on 4/10/03 to add CullFaceAttrib::reverse. // Bumped to minor version 1 on 4/10/03 to add CullFaceAttrib::reverse. // Bumped to minor version 2 on 4/12/03 to add num_components to texture. @@ -49,6 +49,7 @@ static const unsigned short _bam_minor_ver = 13; // Bumped to minor version 11 on 07/26/04 to add multitexture pointers. // Bumped to minor version 12 on 09/22/04 to add PandaNode::into_collide_mask. // Bumped to minor version 13 on 09/24/04 to store actual LODNode switch distances instead of squares. +// Bumped to minor version 14 on 11/18/04 to differentiate old_hpr from new_hpr in compressed anim channels. #endif