From 1281875cbc871fd56abf1b3efe01c828f37bf5af Mon Sep 17 00:00:00 2001 From: rdb Date: Wed, 29 Jan 2025 16:04:53 +0100 Subject: [PATCH] linmath: Fix degenerate case in decompose_matrix Regression in 583f7366db786ff750d77fb8ff09fd3c5718db1c --- panda/src/linmath/compose_matrix_src.cxx | 220 ++++++++++++----------- tests/linmath/test_compose_matrix.py | 17 ++ 2 files changed, 130 insertions(+), 107 deletions(-) diff --git a/panda/src/linmath/compose_matrix_src.cxx b/panda/src/linmath/compose_matrix_src.cxx index c3bb401c89..e5694b7119 100644 --- a/panda/src/linmath/compose_matrix_src.cxx +++ b/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); + if (xz.normalize()) { + // Compute the rotation about the +Y (up) axis. This is yaw, or "heading". + heading = catan2(xz[0], xz[1]); - // 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]; - // 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(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]; - 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; + 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); + if (yz.normalize()) { + // Compute the rotation about the +X (right) axis. This is pitch. + pitch = -catan2(yz[0], yz[1]); - // 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; - // 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(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]; - 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; + 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); + if (xy.normalize()) { + // Compute the rotation about the +Z (back) axis. This is roll. + roll = -catan2(xy[1], xy[0]); - // 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; - // 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(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; - 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; + 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); + if (xy.normalize()) { + // Compute the rotation about the +Z (up) axis. This is yaw, or "heading". + heading = -catan2(xy[0], xy[1]); - // 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; - // 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(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; - 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; + 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); + if (yz.normalize()) { + // Compute the rotation about the +X (right) axis. This is pitch. + pitch = catan2(yz[1], yz[0]); - // 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; - // 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(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]; - 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; + 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); diff --git a/tests/linmath/test_compose_matrix.py b/tests/linmath/test_compose_matrix.py index 82497cb67d..bd52875a02 100644 --- a/tests/linmath/test_compose_matrix.py +++ b/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) + + +@pytest.mark.parametrize("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))