linmath: Fix degenerate case in decompose_matrix

Regression in 583f7366db786ff750d77fb8ff09fd3c5718db1c

panda/src/linmath/compose_matrix_src.cxx

@@ -323,79 +323,82 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
   mat.get_row(z,2);
 
   // Project Z into the XZ plane.
+  FLOATTYPE heading = 0;
   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 = catan2(xz[0], xz[1]);
-
-  // Unwind the heading, and continue.
-  FLOATNAME(LMatrix3) rot_y;
-  rot_y._m(0, 0) = xz[1];
-  rot_y._m(0, 1) = 0;
-  rot_y._m(0, 2) = xz[0];
-
-  rot_y._m(1, 0) = 0;
-  rot_y._m(1, 1) = 1;
-  rot_y._m(1, 2) = 0;
-
-  rot_y._m(2, 0) = -xz[0];
-  rot_y._m(2, 1) = 0;
-  rot_y._m(2, 2) = xz[1];
-
-  x = x * rot_y;
-  y = y * rot_y;
-  z = z * rot_y;
+  if (xz.normalize()) {
+    // Compute the rotation about the +Y (up) axis.  This is yaw, or "heading".
+    heading = catan2(xz[0], xz[1]);
+
+    // Unwind the heading, and continue.
+    FLOATNAME(LMatrix3) rot_y;
+    rot_y._m(0, 0) = xz[1];
+    rot_y._m(0, 1) = 0;
+    rot_y._m(0, 2) = xz[0];
+
+    rot_y._m(1, 0) = 0;
+    rot_y._m(1, 1) = 1;
+    rot_y._m(1, 2) = 0;
+
+    rot_y._m(2, 0) = -xz[0];
+    rot_y._m(2, 1) = 0;
+    rot_y._m(2, 2) = xz[1];
+
+    x = x * rot_y;
+    y = y * rot_y;
+    z = z * rot_y;
+  }
 
   // Project the rotated Z into the YZ plane.
+  FLOATTYPE pitch = 0;
   FLOATNAME(LVector2) yz(z[1], z[2]);
-  yz = normalize(yz);
-
-  // Compute the rotation about the +X (right) axis.  This is pitch.
-  FLOATTYPE pitch = -catan2(yz[0], yz[1]);
-
-  // Unwind the pitch.
-  FLOATNAME(LMatrix3) rot_x;
-  rot_x._m(0, 0) = 1;
-  rot_x._m(0, 1) = 0;
-  rot_x._m(0, 2) = 0;
-
-  rot_x._m(1, 0) = 0;
-  rot_x._m(1, 1) = yz[1];
-  rot_x._m(1, 2) = yz[0];
-
-  rot_x._m(2, 0) = 0;
-  rot_x._m(2, 1) = -yz[0];
-  rot_x._m(2, 2) = yz[1];
-
-  x = x * rot_x;
-  y = y * rot_x;
-  z = z * rot_x;
+  if (yz.normalize()) {
+    // Compute the rotation about the +X (right) axis.  This is pitch.
+    pitch = -catan2(yz[0], yz[1]);
+
+    // Unwind the pitch.
+    FLOATNAME(LMatrix3) rot_x;
+    rot_x._m(0, 0) = 1;
+    rot_x._m(0, 1) = 0;
+    rot_x._m(0, 2) = 0;
+
+    rot_x._m(1, 0) = 0;
+    rot_x._m(1, 1) = yz[1];
+    rot_x._m(1, 2) = yz[0];
+
+    rot_x._m(2, 0) = 0;
+    rot_x._m(2, 1) = -yz[0];
+    rot_x._m(2, 2) = yz[1];
+
+    x = x * rot_x;
+    y = y * rot_x;
+    z = z * rot_x;
+  }
 
   // Project the rotated X onto the XY plane.
+  FLOATTYPE roll = 0;
   FLOATNAME(LVector2) xy(x[0], x[1]);
-  xy = normalize(xy);
-
-  // Compute the rotation about the +Z (back) axis.  This is roll.
-  FLOATTYPE roll = -catan2(xy[1], xy[0]);
-
-  // Unwind the roll from the axes, and continue.
-  FLOATNAME(LMatrix3) rot_z;
-  rot_z._m(0, 0) = xy[0];
-  rot_z._m(0, 1) = -xy[1];
-  rot_z._m(0, 2) = 0;
-
-  rot_z._m(1, 0) = xy[1];
-  rot_z._m(1, 1) = xy[0];
-  rot_z._m(1, 2) = 0;
-
-  rot_z._m(2, 0) = 0;
-  rot_z._m(2, 1) = 0;
-  rot_z._m(2, 2) = 1;
-
-  x = x * rot_z;
-  y = y * rot_z;
-  z = z * rot_z;
+  if (xy.normalize()) {
+    // Compute the rotation about the +Z (back) axis.  This is roll.
+    roll = -catan2(xy[1], xy[0]);
+
+    // Unwind the roll from the axes, and continue.
+    FLOATNAME(LMatrix3) rot_z;
+    rot_z._m(0, 0) = xy[0];
+    rot_z._m(0, 1) = -xy[1];
+    rot_z._m(0, 2) = 0;
+
+    rot_z._m(1, 0) = xy[1];
+    rot_z._m(1, 1) = xy[0];
+    rot_z._m(1, 2) = 0;
+
+    rot_z._m(2, 0) = 0;
+    rot_z._m(2, 1) = 0;
+    rot_z._m(2, 2) = 1;
+
+    x = x * rot_z;
+    y = y * rot_z;
+    z = z * rot_z;
+  }
 
   // Reset the matrix to reflect the unwinding.
   mat.set_row(0, x);
@@ -425,79 +428,82 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
   mat.get_row(z,2);
 
   // Project Y into the XY plane.
+  FLOATTYPE heading = 0;
   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 = -catan2(xy[0], xy[1]);
-
-  // Unwind the heading, and continue.
-  FLOATNAME(LMatrix3) rot_z;
-  rot_z._m(0, 0) = xy[1];
-  rot_z._m(0, 1) = xy[0];
-  rot_z._m(0, 2) = 0;
-
-  rot_z._m(1, 0) = -xy[0];
-  rot_z._m(1, 1) = xy[1];
-  rot_z._m(1, 2) = 0;
-
-  rot_z._m(2, 0) = 0;
-  rot_z._m(2, 1) = 0;
-  rot_z._m(2, 2) = 1;
-
-  x = x * rot_z;
-  y = y * rot_z;
-  z = z * rot_z;
+  if (xy.normalize()) {
+    // Compute the rotation about the +Z (up) axis.  This is yaw, or "heading".
+    heading = -catan2(xy[0], xy[1]);
+
+    // Unwind the heading, and continue.
+    FLOATNAME(LMatrix3) rot_z;
+    rot_z._m(0, 0) = xy[1];
+    rot_z._m(0, 1) = xy[0];
+    rot_z._m(0, 2) = 0;
+
+    rot_z._m(1, 0) = -xy[0];
+    rot_z._m(1, 1) = xy[1];
+    rot_z._m(1, 2) = 0;
+
+    rot_z._m(2, 0) = 0;
+    rot_z._m(2, 1) = 0;
+    rot_z._m(2, 2) = 1;
+
+    x = x * rot_z;
+    y = y * rot_z;
+    z = z * rot_z;
+  }
 
   // Project the rotated Y into the YZ plane.
+  FLOATTYPE pitch = 0;
   FLOATNAME(LVector2) yz(y[1], y[2]);
-  yz = normalize(yz);
-
-  // Compute the rotation about the +X (right) axis.  This is pitch.
-  FLOATTYPE pitch = catan2(yz[1], yz[0]);
-
-  // Unwind the pitch.
-  FLOATNAME(LMatrix3) rot_x;
-  rot_x._m(0, 0) = 1;
-  rot_x._m(0, 1) = 0;
-  rot_x._m(0, 2) = 0;
-
-  rot_x._m(1, 0) = 0;
-  rot_x._m(1, 1) = yz[0];
-  rot_x._m(1, 2) = -yz[1];
-
-  rot_x._m(2, 0) = 0;
-  rot_x._m(2, 1) = yz[1];
-  rot_x._m(2, 2) = yz[0];
-
-  x = x * rot_x;
-  y = y * rot_x;
-  z = z * rot_x;
+  if (yz.normalize()) {
+    // Compute the rotation about the +X (right) axis.  This is pitch.
+    pitch = catan2(yz[1], yz[0]);
+
+    // Unwind the pitch.
+    FLOATNAME(LMatrix3) rot_x;
+    rot_x._m(0, 0) = 1;
+    rot_x._m(0, 1) = 0;
+    rot_x._m(0, 2) = 0;
+
+    rot_x._m(1, 0) = 0;
+    rot_x._m(1, 1) = yz[0];
+    rot_x._m(1, 2) = -yz[1];
+
+    rot_x._m(2, 0) = 0;
+    rot_x._m(2, 1) = yz[1];
+    rot_x._m(2, 2) = yz[0];
+
+    x = x * rot_x;
+    y = y * rot_x;
+    z = z * rot_x;
+  }
 
   // Project X into the XZ plane.
+  FLOATTYPE roll = 0;
   FLOATNAME(LVector2) xz(x[0], x[2]);
-  xz = normalize(xz);
-
+  if (xz.normalize()) {
   // Compute the rotation about the -Y (back) axis.  This is roll.
-  FLOATTYPE roll = -catan2(xz[1], xz[0]);
+    roll = -catan2(xz[1], xz[0]);
 
-  // Unwind the roll from the axes, and continue.
-  FLOATNAME(LMatrix3) rot_y;
-  rot_y._m(0, 0) = xz[0];
-  rot_y._m(0, 1) = 0;
-  rot_y._m(0, 2) = -xz[1];
+    // Unwind the roll from the axes, and continue.
+    FLOATNAME(LMatrix3) rot_y;
+    rot_y._m(0, 0) = xz[0];
+    rot_y._m(0, 1) = 0;
+    rot_y._m(0, 2) = -xz[1];
 
-  rot_y._m(1, 0) = 0;
-  rot_y._m(1, 1) = 1;
-  rot_y._m(1, 2) = 0;
+    rot_y._m(1, 0) = 0;
+    rot_y._m(1, 1) = 1;
+    rot_y._m(1, 2) = 0;
 
-  rot_y._m(2, 0) = xz[1];
-  rot_y._m(2, 1) = 0;
-  rot_y._m(2, 2) = xz[0];
+    rot_y._m(2, 0) = xz[1];
+    rot_y._m(2, 1) = 0;
+    rot_y._m(2, 2) = xz[0];
 
-  x = x * rot_y;
-  y = y * rot_y;
-  z = z * rot_y;
+    x = x * rot_y;
+    y = y * rot_y;
+    z = z * rot_y;
+  }
 
   // Reset the matrix to reflect the unwinding.
   mat.set_row(0, x);

tests/linmath/test_compose_matrix.py

@@ -24,3 +24,20 @@ def test_compose_matrix(coordsys):
     new_quat = core.LQuaternion()
     new_quat.set_hpr(new_hpr, coordsys)
     assert quat.is_same_direction(new_quat)
+
+
[email protected]("coordsys", (core.CS_zup_right, core.CS_yup_right, core.CS_zup_left, core.CS_yup_left))
+def test_compose_matrix2(coordsys):
+    mat = core.LMatrix3(1, 0, 0, 0, 0, -1, 0, 1, 0)
+
+    new_scale = core.LVecBase3()
+    new_hpr = core.LVecBase3()
+    new_shear = core.LVecBase3()
+    core.decompose_matrix(mat, new_scale, new_shear, new_hpr, coordsys)
+
+    assert new_scale.almost_equal(core.LVecBase3(1, 1, 1))
+    if coordsys in (core.CS_zup_left, core.CS_yup_left):
+        assert new_hpr.almost_equal(core.LVecBase3(0, 90, 0))
+    else:
+        assert new_hpr.almost_equal(core.LVecBase3(0, -90, 0))
+    assert new_shear.almost_equal(core.LVecBase3(0, 0, 0))