diff --git a/src/systemcmds/tests/test_matrix.cpp b/src/systemcmds/tests/test_matrix.cpp index 44c71655a2..654c019157 100644 --- a/src/systemcmds/tests/test_matrix.cpp +++ b/src/systemcmds/tests/test_matrix.cpp @@ -29,6 +29,7 @@ private: bool vector2Tests(); bool vector3Tests(); bool vectorAssignmentTests(); + bool dcmRenormTests(); }; bool MatrixTest::run_tests(void) @@ -49,6 +50,7 @@ bool MatrixTest::run_tests(void) ut_run_test(vector2Tests); ut_run_test(vector3Tests); ut_run_test(vectorAssignmentTests); + ut_run_test(dcmRenormTests); return (_tests_failed == 0); } @@ -677,3 +679,46 @@ bool MatrixTest::vectorAssignmentTests(void) return true; } + +bool MatrixTest::dcmRenormTests(void) +{ + bool verbose = false; + + Dcm A = eye(); + Euler euler(0.1f, 0.2f, 0.3f); + Dcm R(euler); + + // demonstrate need for renormalization + for (int i = 0; i < 1000; i++) { + A = R * A; + } + + float err = 0.0f; + + if (verbose) { + for (int row = 0; row < 3; row++) { + matrix::Vector3f rvec(A._data[row]); + err += fabsf(1.0f - rvec.length()); + } + + printf("error: %e\n", (double)err); + } + + A.renormalize(); + + err = 0.0f; + + for (int row = 0; row < 3; row++) { + matrix::Vector3f rvec(A._data[row]); + err += fabsf(1.0f - rvec.length()); + } + + if (verbose) { + printf("renorm error: %e\n", (double)err); + } + + static const float eps = 1e-7f; + ut_test(err < eps); + + return true; +}