From 17f7292fe9af3c590d21265be6891c1f575bb126 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 19 Mar 2012 17:23:22 +1100 Subject: [PATCH] Math: added a test for the rotate() method --- libraries/AP_Math/examples/eulers/eulers.pde | 47 ++++++++++++++++++++ 1 file changed, 47 insertions(+) diff --git a/libraries/AP_Math/examples/eulers/eulers.pde b/libraries/AP_Math/examples/eulers/eulers.pde index 1cd6581923..3614e472a9 100644 --- a/libraries/AP_Math/examples/eulers/eulers.pde +++ b/libraries/AP_Math/examples/eulers/eulers.pde @@ -212,6 +212,52 @@ void test_frame_transforms(void) printf("%f %f %f\n", v2.x, v2.y, v2.z); } +// generate a random float between -1 and 1 +static float rand_num(void) +{ + float ret = ((unsigned)random()) % 2000000; + return (ret - 1.0e6) / 1.0e6; +} + +void test_matrix_rotate(void) +{ + Matrix3f m1, m2, diff; + Vector3f r; + + m1.identity(); + m2.identity(); + r.x = rand_num(); + r.y = rand_num(); + r.z = rand_num(); + + for (uint16_t i = 0; i<1000; i++) { + // old method + Matrix3f temp_matrix; + temp_matrix.a.x = 0; + temp_matrix.a.y = -r.z; + temp_matrix.a.z = r.y; + temp_matrix.b.x = r.z; + temp_matrix.b.y = 0; + temp_matrix.b.z = -r.x; + temp_matrix.c.x = -r.y; + temp_matrix.c.y = r.x; + temp_matrix.c.z = 0; + temp_matrix = m1 * temp_matrix; + m1 += temp_matrix; + + // new method + m2.rotate(r); + + // check they behave in the same way + diff = m1 - m2; + float err = diff.a.length() + diff.b.length() + diff.c.length(); + + if (err > 0) { + Serial.printf("ERROR: i=%u err=%f\n", (unsigned)i, err); + } + } +} + /* euler angle tests */ @@ -226,6 +272,7 @@ void setup(void) test_conversions(); test_quaternion_eulers(); test_matrix_eulers(); + test_matrix_rotate(); } void