debug messages

panda/src/linmath/compose_matrix_src.cxx

@@ -27,19 +27,20 @@ compose_matrix(FLOATNAME(LMatrix3) &mat,
                const FLOATNAME(LVecBase3) &hpr,
                CoordinateSystem cs) {
 
-  // temp_hpr_fix blocks use the correct way.  need to keep other way as default until
-  // legacy tools are fixed to work with correct way
+  // temp_hpr_fix blocks use the correct way.  need to keep other way
+  // as default until legacy tools are fixed to work with correct way
 
   if (temp_hpr_fix) {
     mat.scale_multiply(scale,
-      FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[2], FLOATNAME(LVector3)::forward(cs), cs) *
-      FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[1], FLOATNAME(LVector3)::right(cs), cs) *
-      FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[0], FLOATNAME(LVector3)::up(cs), cs));
+                       FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[2], FLOATNAME(LVector3)::forward(cs), cs) *
+                       FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[1], FLOATNAME(LVector3)::right(cs), cs) *
+                       FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[0], FLOATNAME(LVector3)::up(cs), cs));
+
   } else {
     mat.scale_multiply(scale,
-      FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[1], FLOATNAME(LVector3)::right(cs), cs) *
-      FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[0], FLOATNAME(LVector3)::up(cs), cs) *
-      FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[2], FLOATNAME(LVector3)::back(cs), cs));
+                       FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[1], FLOATNAME(LVector3)::right(cs), cs) *
+                       FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[0], FLOATNAME(LVector3)::up(cs), cs) *
+                       FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[2], FLOATNAME(LVector3)::back(cs), cs));
   }
 }
 
@@ -75,7 +76,7 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
     // Unwind the heading, and continue.
     Matrix rot_y;
     rot_y = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f),
-                               CS_yup_right);
+                                        CS_yup_right);
 
     x = x * rot_y;
     y = y * rot_y;
@@ -91,7 +92,7 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
     // Unwind the pitch.
     Matrix rot_x;
     rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f),
-                               CS_yup_right);
+                                        CS_yup_right);
 
     x = x * rot_x;
     y = y * rot_x;
@@ -107,7 +108,7 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
     // Unwind the roll from the axes, and continue.
     Matrix rot_z;
     rot_z = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f),
-                               CS_yup_right);
+                                        CS_yup_right);
 
     x = x * rot_z;
     y = y * rot_z;
@@ -140,7 +141,7 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
     // Unwind the roll from the axes, and continue.
     Matrix rot_z;
     rot_z = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f),
-                               CS_yup_right);
+                                        CS_yup_right);
 
     x = x * rot_z;
     y = y * rot_z;
@@ -157,7 +158,7 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
     // Unwind the heading, and continue.
     Matrix rot_y;
     rot_y = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f),
-                               CS_yup_right);
+                                        CS_yup_right);
 
     x = x * rot_y;
     y = y * rot_y;
@@ -173,7 +174,7 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
     // Unwind the pitch.
     Matrix rot_x;
     rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f),
-                               CS_yup_right);
+                                        CS_yup_right);
 
     x = x * rot_x;
     y = y * rot_x;
@@ -220,7 +221,7 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr,
   // Unwind the roll from the axes, and continue.
   Matrix rot_z;
   rot_z = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f),
-                             CS_yup_right);
+                                      CS_yup_right);
 
   x = x * rot_z;
   y = y * rot_z;
@@ -237,7 +238,7 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr,
   // Unwind the heading, and continue.
   Matrix rot_y;
   rot_y = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f),
-                             CS_yup_right);
+                                      CS_yup_right);
 
   x = x * rot_y;
   y = y * rot_y;
@@ -253,7 +254,7 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr,
   // Unwind the pitch.
   Matrix rot_x;
   rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f),
-                             CS_yup_right);
+                                      CS_yup_right);
 
   x = x * rot_x;
   y = y * rot_x;
@@ -301,7 +302,7 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
     // Unwind the heading, and continue.
     Matrix rot_z;
     rot_z = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f),
-                               CS_zup_right);
+                                        CS_zup_right);
 
     x = x * rot_z;
     y = y * rot_z;
@@ -317,7 +318,7 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
     // Unwind the pitch.
     Matrix rot_x;
     rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f),
-                               CS_zup_right);
+                                        CS_zup_right);
 
     x = x * rot_x;
     y = y * rot_x;
@@ -333,7 +334,7 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
     // Unwind the roll from the axes, and continue.
     Matrix rot_y;
     rot_y = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f),
-                               CS_zup_right);
+                                        CS_zup_right);
 
     x = x * rot_y;
     y = y * rot_y;
@@ -377,7 +378,7 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
     // Unwind the roll from the axes, and continue.
     Matrix rot_y;
     rot_y = Matrix::rotate_mat_normaxis(roll, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f),
-                               CS_zup_right);
+                                        CS_zup_right);
 
     x = x * rot_y;
     y = y * rot_y;
@@ -394,7 +395,7 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
     // Unwind the heading, and continue.
     Matrix rot_z;
     rot_z = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f),
-                               CS_zup_right);
+                                        CS_zup_right);
 
     x = x * rot_z;
     y = y * rot_z;
@@ -410,7 +411,7 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
     // Unwind the pitch.
     Matrix rot_x;
     rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f),
-                               CS_zup_right);
+                                        CS_zup_right);
 
     x = x * rot_x;
     y = y * rot_x;
@@ -457,7 +458,7 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr,
   // Unwind the roll from the axes, and continue.
   Matrix rot_y;
   rot_y = Matrix::rotate_mat_normaxis(roll, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f),
-                             CS_zup_right);
+                                      CS_zup_right);
 
   x = x * rot_y;
   y = y * rot_y;
@@ -474,7 +475,7 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr,
   // Unwind the heading, and continue.
   Matrix rot_z;
   rot_z = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f),
-                             CS_zup_right);
+                                      CS_zup_right);
 
   x = x * rot_z;
   y = y * rot_z;
@@ -490,7 +491,7 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr,
   // Unwind the pitch.
   Matrix rot_x;
   rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f),
-                             CS_zup_right);
+                                      CS_zup_right);
 
   x = x * rot_x;
   y = y * rot_x;
@@ -523,6 +524,11 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat,
     cs = default_coordinate_system;
   }
 
+  if (linmath_cat.is_debug()) {
+    linmath_cat.debug()
+      << "decomposing " << mat << " via cs " << cs << "\n";
+  }
+
   // Extract the rotation and scale, according to the coordinate
   // system of choice.
   bool bMatHasNoShear,bIsLeftHandedMat;
@@ -533,46 +539,46 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat,
   case CS_zup_right:
     {
       unwind_zup_rotation(new_mat, hpr);
-          bIsLeftHandedMat = false;
+      bIsLeftHandedMat = false;
     }
     break;
 
   case CS_yup_right:
     {
       unwind_yup_rotation(new_mat, hpr);
-          bIsLeftHandedMat = false;
-        }
+      bIsLeftHandedMat = false;
+    }
     break;
 
   case CS_zup_left:
     {
-          new_mat._m.m._02 = -new_mat._m.m._02;
-          new_mat._m.m._12 = -new_mat._m.m._12;
-          new_mat._m.m._20 = -new_mat._m.m._20;
-          new_mat._m.m._21 = -new_mat._m.m._21;
-/*
-      FLOATNAME(LMatrix3) lm(mat(0, 0), mat(0, 1), -mat(0, 2),
-                           mat(1, 0), mat(1, 1), -mat(1, 2),
-                           -mat(2, 0), -mat(2, 1), mat(2, 2));
-*/
+      new_mat._m.m._02 = -new_mat._m.m._02;
+      new_mat._m.m._12 = -new_mat._m.m._12;
+      new_mat._m.m._20 = -new_mat._m.m._20;
+      new_mat._m.m._21 = -new_mat._m.m._21;
+      /*
+        FLOATNAME(LMatrix3) lm(mat(0, 0), mat(0, 1), -mat(0, 2),
+        mat(1, 0), mat(1, 1), -mat(1, 2),
+        -mat(2, 0), -mat(2, 1), mat(2, 2));
+      */
       unwind_zup_rotation(new_mat, hpr);
-          bIsLeftHandedMat = true;
+      bIsLeftHandedMat = true;
     }
     break;
 
   case CS_yup_left:
     {
-          new_mat._m.m._02 = -new_mat._m.m._02;
-          new_mat._m.m._12 = -new_mat._m.m._12;
-          new_mat._m.m._20 = -new_mat._m.m._20;
-          new_mat._m.m._21 = -new_mat._m.m._21;
-/*
-      FLOATNAME(LMatrix3) lm(mat(0, 0), mat(0, 1), -mat(0, 2),
-                           mat(1, 0), mat(1, 1), -mat(1, 2),
-                           -mat(2, 0), -mat(2, 1), mat(2, 2));
-*/
+      new_mat._m.m._02 = -new_mat._m.m._02;
+      new_mat._m.m._12 = -new_mat._m.m._12;
+      new_mat._m.m._20 = -new_mat._m.m._20;
+      new_mat._m.m._21 = -new_mat._m.m._21;
+      /*
+        FLOATNAME(LMatrix3) lm(mat(0, 0), mat(0, 1), -mat(0, 2),
+        mat(1, 0), mat(1, 1), -mat(1, 2),
+        -mat(2, 0), -mat(2, 1), mat(2, 2));
+      */
       unwind_yup_rotation(new_mat, hpr);
-          bIsLeftHandedMat = true;
+      bIsLeftHandedMat = true;
     }
     break;
 
@@ -581,21 +587,26 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat,
       << "Unexpected coordinate system: " << (int)cs << "\n";
     return false;
   }
+  
+  if (linmath_cat.is_debug()) {
+    linmath_cat.debug()
+      << "after unwind, mat is " << new_mat << "\n";
+  }
 
-   scale[2] = new_mat._m.m._22;
-   if(bIsLeftHandedMat) {
-           scale[0] = -new_mat._m.m._00;
-           scale[1] = -new_mat._m.m._11;
-   } else {
-           scale[0] = new_mat._m.m._00;
-           scale[1] = new_mat._m.m._11;
-   }
-
-   bMatHasNoShear =
-        (fabs(new_mat(0, 1)) + fabs(new_mat(0, 2)) +
-         fabs(new_mat(1, 0)) + fabs(new_mat(1, 2)) +
-         fabs(new_mat(2, 0)) + fabs(new_mat(2, 1))) < 0.0001;
-
+  scale[2] = new_mat._m.m._22;
+  if(bIsLeftHandedMat) {
+    scale[0] = -new_mat._m.m._00;
+    scale[1] = -new_mat._m.m._11;
+  } else {
+    scale[0] = new_mat._m.m._00;
+    scale[1] = new_mat._m.m._11;
+  }
+  
+  bMatHasNoShear =
+    (fabs(new_mat(0, 1)) + fabs(new_mat(0, 2)) +
+     fabs(new_mat(1, 0)) + fabs(new_mat(1, 2)) +
+     fabs(new_mat(2, 0)) + fabs(new_mat(2, 1))) < 0.0001;
+  
   return bMatHasNoShear;
 }
 
@@ -632,46 +643,46 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat,
   case CS_zup_right:
     {
       unwind_zup_rotation(new_mat, hpr, roll);
-          bIsLeftHandedMat = false;
+      bIsLeftHandedMat = false;
     }
     break;
 
   case CS_yup_right:
     {
       unwind_yup_rotation(new_mat, hpr, roll);
-          bIsLeftHandedMat = false;
-        }
+      bIsLeftHandedMat = false;
+    }
     break;
 
   case CS_zup_left:
     {
-          new_mat._m.m._02 = -new_mat._m.m._02;
-          new_mat._m.m._12 = -new_mat._m.m._12;
-          new_mat._m.m._20 = -new_mat._m.m._20;
-          new_mat._m.m._21 = -new_mat._m.m._21;
-/*
-      FLOATNAME(LMatrix3) lm(mat(0, 0), mat(0, 1), -mat(0, 2),
-                           mat(1, 0), mat(1, 1), -mat(1, 2),
-                           -mat(2, 0), -mat(2, 1), mat(2, 2));
-*/
+      new_mat._m.m._02 = -new_mat._m.m._02;
+      new_mat._m.m._12 = -new_mat._m.m._12;
+      new_mat._m.m._20 = -new_mat._m.m._20;
+      new_mat._m.m._21 = -new_mat._m.m._21;
+      /*
+        FLOATNAME(LMatrix3) lm(mat(0, 0), mat(0, 1), -mat(0, 2),
+        mat(1, 0), mat(1, 1), -mat(1, 2),
+        -mat(2, 0), -mat(2, 1), mat(2, 2));
+      */
       unwind_zup_rotation(new_mat, hpr, roll);
-          bIsLeftHandedMat = true;
+      bIsLeftHandedMat = true;
     }
     break;
 
   case CS_yup_left:
     {
-          new_mat._m.m._02 = -new_mat._m.m._02;
-          new_mat._m.m._12 = -new_mat._m.m._12;
-          new_mat._m.m._20 = -new_mat._m.m._20;
-          new_mat._m.m._21 = -new_mat._m.m._21;
-/*
-      FLOATNAME(LMatrix3) lm(mat(0, 0), mat(0, 1), -mat(0, 2),
-                           mat(1, 0), mat(1, 1), -mat(1, 2),
-                           -mat(2, 0), -mat(2, 1), mat(2, 2));
-*/
+      new_mat._m.m._02 = -new_mat._m.m._02;
+      new_mat._m.m._12 = -new_mat._m.m._12;
+      new_mat._m.m._20 = -new_mat._m.m._20;
+      new_mat._m.m._21 = -new_mat._m.m._21;
+      /*
+        FLOATNAME(LMatrix3) lm(mat(0, 0), mat(0, 1), -mat(0, 2),
+        mat(1, 0), mat(1, 1), -mat(1, 2),
+        -mat(2, 0), -mat(2, 1), mat(2, 2));
+      */
       unwind_yup_rotation(new_mat, hpr, roll);
-          bIsLeftHandedMat = true;
+      bIsLeftHandedMat = true;
     }
     break;
 
@@ -681,19 +692,19 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat,
     return false;
   }
 
-   scale[2] = new_mat._m.m._22;
-   if(bIsLeftHandedMat) {
-           scale[0] = -new_mat._m.m._00;
-           scale[1] = -new_mat._m.m._11;
-   } else {
-           scale[0] = new_mat._m.m._00;
-           scale[1] = new_mat._m.m._11;
-   }
-
-   bMatHasNoShear =
-        (fabs(new_mat(0, 1)) + fabs(new_mat(0, 2)) +
-         fabs(new_mat(1, 0)) + fabs(new_mat(1, 2)) +
-         fabs(new_mat(2, 0)) + fabs(new_mat(2, 1))) < 0.0001;
+  scale[2] = new_mat._m.m._22;
+  if(bIsLeftHandedMat) {
+    scale[0] = -new_mat._m.m._00;
+    scale[1] = -new_mat._m.m._11;
+  } else {
+    scale[0] = new_mat._m.m._00;
+    scale[1] = new_mat._m.m._11;
+  }
+
+  bMatHasNoShear =
+    (fabs(new_mat(0, 1)) + fabs(new_mat(0, 2)) +
+     fabs(new_mat(1, 0)) + fabs(new_mat(1, 2)) +
+     fabs(new_mat(2, 0)) + fabs(new_mat(2, 1))) < 0.0001;
 
   return bMatHasNoShear;
 }