diff options
Diffstat (limited to 'src/systemcmds/tests/test_mathlib.cpp')
| -rw-r--r-- | src/systemcmds/tests/test_mathlib.cpp | 47 |
1 files changed, 26 insertions, 21 deletions
diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index a66cebd2f..3a890c30b 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -284,11 +284,11 @@ int test_mathlib(int argc, char *argv[]) { // test quaternion method "rotate" (rotate vector by quaternion) - Vector<3> vector = {1.0f,1.0f,1.0f}; + Vector<3> vector = {1.0f, 1.0f, 1.0f}; Vector<3> vector_q; Vector<3> vector_r; Quaternion q; - Matrix<3,3> R; + Matrix<3, 3> R; float diff = 0.1f; float tol = 0.00001f; @@ -298,11 +298,12 @@ int test_mathlib(int argc, char *argv[]) for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) { R.from_euler(roll, pitch, yaw); - q.from_euler(roll,pitch,yaw); - vector_r = R*vector; + q.from_euler(roll, pitch, yaw); + vector_r = R * vector; vector_q = q.rotate(vector); + for (int i = 0; i < 3; i++) { - if(fabsf(vector_r(i) - vector_q(i)) > tol) { + if (fabsf(vector_r(i) - vector_q(i)) > tol) { warnx("Quaternion method 'rotate' outside tolerance"); rc = 1; } @@ -313,41 +314,45 @@ int test_mathlib(int argc, char *argv[]) // test some values calculated with matlab tol = 0.0001f; - q.from_euler(M_PI_2_F,0.0f,0.0f); + q.from_euler(M_PI_2_F, 0.0f, 0.0f); vector_q = q.rotate(vector); - Vector<3> vector_true = {1.00f,-1.00f,1.00f}; - for(unsigned i = 0;i<3;i++) { - if(fabsf(vector_true(i) - vector_q(i)) > tol) { + Vector<3> vector_true = {1.00f, -1.00f, 1.00f}; + + for (unsigned i = 0; i < 3; i++) { + if (fabsf(vector_true(i) - vector_q(i)) > tol) { warnx("Quaternion method 'rotate' outside tolerance"); rc = 1; } } - q.from_euler(0.3f,0.2f,0.1f); + q.from_euler(0.3f, 0.2f, 0.1f); vector_q = q.rotate(vector); - vector_true = {1.1566,0.7792,1.0273}; - for(unsigned i = 0;i<3;i++) { - if(fabsf(vector_true(i) - vector_q(i)) > tol) { + vector_true = {1.1566, 0.7792, 1.0273}; + + for (unsigned i = 0; i < 3; i++) { + if (fabsf(vector_true(i) - vector_q(i)) > tol) { warnx("Quaternion method 'rotate' outside tolerance"); rc = 1; } } - q.from_euler(-1.5f,-0.2f,0.5f); + q.from_euler(-1.5f, -0.2f, 0.5f); vector_q = q.rotate(vector); - vector_true = {0.5095,1.4956,-0.7096}; - for(unsigned i = 0;i<3;i++) { - if(fabsf(vector_true(i) - vector_q(i)) > tol) { + vector_true = {0.5095, 1.4956, -0.7096}; + + for (unsigned i = 0; i < 3; i++) { + if (fabsf(vector_true(i) - vector_q(i)) > tol) { warnx("Quaternion method 'rotate' outside tolerance"); rc = 1; } } - q.from_euler(M_PI_2_F,-M_PI_2_F,-M_PI_F/3.0f); + q.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f); vector_q = q.rotate(vector); - vector_true = {-1.3660,0.3660,1.0000}; - for(unsigned i = 0;i<3;i++) { - if(fabsf(vector_true(i) - vector_q(i)) > tol) { + vector_true = { -1.3660, 0.3660, 1.0000}; + + for (unsigned i = 0; i < 3; i++) { + if (fabsf(vector_true(i) - vector_q(i)) > tol) { warnx("Quaternion method 'rotate' outside tolerance"); rc = 1; } |