linmath: Significant perf optimizations for decompose_matrix

panda/src/linmath/compose_matrix_src.cxx

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

tests/linmath/test_compose_matrix.py

@@ -0,0 +1,26 @@
+from panda3d import core
+import pytest
+
+
[email protected]("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)