RomanNum3ral
/
open_toontown_panda3d
mirror of https://github.com/open-toontown/panda3d.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

linmath: Fix degenerate case in decompose_matrix 

Regression in 583f7366db
This commit is contained in:
rdb 2025-01-29 16:04:53 +01:00
parent 1fa8e2b0fc
commit 1281875cbc
2 changed files with 130 additions and 107 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

220
panda/src/linmath/compose_matrix_src.cxx
View File

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

17
tests/linmath/test_compose_matrix.py
View File

@ -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))