linmath: Significant perf optimizations for decompose_matrix
This commit is contained in:
rdb
parent
f50fef3744
commit
583f7366db
|
|
@ -327,12 +327,21 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
|
|||
xz = normalize(xz);
|
||||
|
||||
// Compute the rotation about the +Y (up) axis. This is yaw, or "heading".
|
||||
FLOATTYPE heading = rad_2_deg(((FLOATTYPE)catan2(xz[0], xz[1])));
|
||||
FLOATTYPE heading = catan2(xz[0], xz[1]);
|
||||
|
||||
// Unwind the heading, and continue.
|
||||
FLOATNAME(LMatrix3) rot_y;
|
||||
rot_y.set_rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f),
|
||||
CS_yup_right);
|
||||
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;
|
||||
|
|
@ -343,12 +352,21 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
|
|||
yz = normalize(yz);
|
||||
|
||||
// Compute the rotation about the +X (right) axis. This is pitch.
|
||||
FLOATTYPE pitch = rad_2_deg((FLOATTYPE)(-catan2(yz[0], yz[1])));
|
||||
FLOATTYPE pitch = -catan2(yz[0], yz[1]);
|
||||
|
||||
// Unwind the pitch.
|
||||
FLOATNAME(LMatrix3) rot_x;
|
||||
rot_x.set_rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f),
|
||||
CS_yup_right);
|
||||
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;
|
||||
|
|
@ -359,12 +377,21 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
|
|||
xy = normalize(xy);
|
||||
|
||||
// Compute the rotation about the +Z (back) axis. This is roll.
|
||||
FLOATTYPE roll = -rad_2_deg(((FLOATTYPE)catan2(xy[1], xy[0])));
|
||||
FLOATTYPE roll = -catan2(xy[1], xy[0]);
|
||||
|
||||
// Unwind the roll from the axes, and continue.
|
||||
FLOATNAME(LMatrix3) rot_z;
|
||||
rot_z.set_rotate_mat_normaxis(roll, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f),
|
||||
CS_yup_right);
|
||||
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;
|
||||
|
|
@ -376,9 +403,9 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
|
|||
mat.set_row(2, z);
|
||||
|
||||
// Return the three rotation components.
|
||||
hpr[0] = heading;
|
||||
hpr[1] = pitch;
|
||||
hpr[2] = roll;
|
||||
hpr[0] = rad_2_deg(heading);
|
||||
hpr[1] = rad_2_deg(pitch);
|
||||
hpr[2] = rad_2_deg(roll);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -402,12 +429,21 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
|
|||
xy = normalize(xy);
|
||||
|
||||
// Compute the rotation about the +Z (up) axis. This is yaw, or "heading".
|
||||
FLOATTYPE heading = -rad_2_deg(((FLOATTYPE)catan2(xy[0], xy[1])));
|
||||
FLOATTYPE heading = -catan2(xy[0], xy[1]);
|
||||
|
||||
// Unwind the heading, and continue.
|
||||
FLOATNAME(LMatrix3) rot_z;
|
||||
rot_z.set_rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f),
|
||||
CS_zup_right);
|
||||
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;
|
||||
|
|
@ -418,12 +454,21 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
|
|||
yz = normalize(yz);
|
||||
|
||||
// Compute the rotation about the +X (right) axis. This is pitch.
|
||||
FLOATTYPE pitch = rad_2_deg(((FLOATTYPE)catan2(yz[1], yz[0])));
|
||||
FLOATTYPE pitch = catan2(yz[1], yz[0]);
|
||||
|
||||
// Unwind the pitch.
|
||||
FLOATNAME(LMatrix3) rot_x;
|
||||
rot_x.set_rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f),
|
||||
CS_zup_right);
|
||||
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;
|
||||
|
|
@ -434,12 +479,21 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
|
|||
xz = normalize(xz);
|
||||
|
||||
// Compute the rotation about the -Y (back) axis. This is roll.
|
||||
FLOATTYPE roll = -rad_2_deg(((FLOATTYPE)catan2(xz[1], xz[0])));
|
||||
FLOATTYPE roll = -catan2(xz[1], xz[0]);
|
||||
|
||||
// Unwind the roll from the axes, and continue.
|
||||
FLOATNAME(LMatrix3) rot_y;
|
||||
rot_y.set_rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f),
|
||||
CS_zup_right);
|
||||
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(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;
|
||||
|
|
@ -451,9 +505,9 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
|
|||
mat.set_row(2, z);
|
||||
|
||||
// Return the three rotation components.
|
||||
hpr[0] = heading;
|
||||
hpr[1] = pitch;
|
||||
hpr[2] = roll;
|
||||
hpr[0] = rad_2_deg(heading);
|
||||
hpr[1] = rad_2_deg(pitch);
|
||||
hpr[2] = rad_2_deg(roll);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -473,10 +527,12 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat,
|
|||
cs = get_default_coordinate_system();
|
||||
}
|
||||
|
||||
#ifdef _DEBUG
|
||||
if (linmath_cat.is_debug()) {
|
||||
linmath_cat.debug()
|
||||
<< "decomposing " << mat << " via cs " << cs << "\n";
|
||||
}
|
||||
#endif
|
||||
|
||||
// Extract the rotation and scale, according to the coordinate system of
|
||||
// choice.
|
||||
|
|
@ -534,10 +590,12 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat,
|
|||
return false;
|
||||
}
|
||||
|
||||
#ifdef _DEBUG
|
||||
if (linmath_cat.is_debug()) {
|
||||
linmath_cat.debug()
|
||||
<< "after unwind, mat is " << new_mat << "\n";
|
||||
}
|
||||
#endif
|
||||
|
||||
scale.set(new_mat(0, 0), new_mat(1, 1), new_mat(2, 2));
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,26 @@
|
|||
from panda3d import core
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.parametrize("coordsys", (core.CS_zup_right, core.CS_yup_right, core.CS_zup_left, core.CS_yup_left))
|
||||
def test_compose_matrix(coordsys):
|
||||
scale = core.LVecBase3(1.2, 0.5, 2)
|
||||
hpr = core.LVecBase3(45, -90, 12.5)
|
||||
shear = core.LVecBase3(0, 0, 0)
|
||||
|
||||
mat = core.LMatrix3()
|
||||
core.compose_matrix(mat, scale, shear, hpr, coordsys)
|
||||
|
||||
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(scale)
|
||||
assert new_shear.almost_equal(shear)
|
||||
|
||||
quat = core.LQuaternion()
|
||||
quat.set_hpr(hpr, coordsys)
|
||||
new_quat = core.LQuaternion()
|
||||
new_quat.set_hpr(new_hpr, coordsys)
|
||||
assert quat.is_same_direction(new_quat)
|
||||
Loading…
Reference in New Issue