separate compose_matrix into old_hpr and new_hpr variants

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());

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,44 +105,36 @@ decompose_matrix(const FLOATNAME(LMatrix4) &mat,
                  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 false;
+  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);
   }
-#ifndef NDEBUG
-  return mat.get_col(3).almost_equal(FLOATNAME(LVecBase4)(0.0, 0.0, 0.0, 1.0));
-#else
-  return true;
-#endif
 }
 
+////////////////////////////////////////////////////////////////////
+//     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) {
-  FLOATNAME(LVector3) scale, shear, hpr, translate;
-  bool result = decompose_matrix(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;
+  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,
@@ -114,11 +143,7 @@ compose_matrix(FLOATNAME(LMatrix3) &mat,
   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.
-////////////////////////////////////////////////////////////////////
+// Deprecated function.
 INLINE_LINMATH void
 compose_matrix(FLOATNAME(LMatrix4) &mat,
                const FLOATNAME(LVecBase3) &scale,
@@ -130,7 +155,8 @@ compose_matrix(FLOATNAME(LMatrix4) &mat,
   mat = FLOATNAME(LMatrix4)(upper3, translate);
 }
 
-INLINE EXPCL_PANDA bool
+// Deprecated function.
+INLINE_LINMATH bool
 decompose_matrix(const FLOATNAME(LMatrix3) &mat,
                  FLOATNAME(LVecBase3) &scale,
                  FLOATNAME(LVecBase3) &hpr,
@@ -142,15 +168,7 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat,
   return shear.almost_equal(FLOATNAME(LVecBase3)::zero());
 }
 
-
-////////////////////////////////////////////////////////////////////
-//     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.
-////////////////////////////////////////////////////////////////////
+// Deprecated function.
 INLINE_LINMATH bool
 decompose_matrix(const FLOATNAME(LMatrix4) &mat,
                  FLOATNAME(LVecBase3) &scale,
@@ -161,3 +179,159 @@ decompose_matrix(const FLOATNAME(LMatrix4) &mat,
   mat.get_row3(translate,3);
   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
+  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_old_hpr(const FLOATNAME(LMatrix4) &mat,
+                         FLOATTYPE components[num_matrix_components],
+                         CoordinateSystem cs) {
+  FLOATNAME(LVector3) scale, shear, hpr, translate;
+  bool result = decompose_matrix_old_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;
+}
+
+// Transitional function.
+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) {
+  FLOATNAME(LMatrix3) upper3;
+  compose_matrix_new_hpr(upper3, scale, shear, hpr, cs);
+  mat = FLOATNAME(LMatrix4)(upper3, translate);
+}
+
+// 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);
+}
+
+// Transitional function.
+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) {
+  // Get the translation first.
+  mat.get_row3(translate,3);
+  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;
+}

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
-//  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.
+//     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 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 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 is deprecated and will soon be removed,
-//               especially when the need for temp_hpr_fix is
-//               eliminated.
+//               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 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));
 
-  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;
+  // 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];
+  }
 
-  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;
+}

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"

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

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();

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