From 1bc836605f65a411d4f0ad6fb82fae947c8c44ad Mon Sep 17 00:00:00 2001 From: David Rose Date: Wed, 2 Jul 2003 19:30:54 +0000 Subject: [PATCH] debug messages --- panda/src/linmath/compose_matrix_src.cxx | 207 ++++++++++++----------- 1 file changed, 109 insertions(+), 98 deletions(-) diff --git a/panda/src/linmath/compose_matrix_src.cxx b/panda/src/linmath/compose_matrix_src.cxx index d4b45140f3..9a3d5b77c8 100644 --- a/panda/src/linmath/compose_matrix_src.cxx +++ b/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; - } + 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; + 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; }