From 4266e924d0c8b57d9032f57cb86eff11de28ca99 Mon Sep 17 00:00:00 2001
From: Gustavo Jose de Sousa <gustavo.sousa@intel.com>
Date: Tue, 3 Nov 2015 10:17:28 -0200
Subject: [PATCH] AP_Math: add unit test for vector rotations

This unit test already shows that rotation is wrong for
ROTATION_YAW_293_PITCH_68_ROLL_90.
---
 libraries/AP_Math/tests/test_vectors.cpp | 61 ++++++++++++++++++++++++
 libraries/AP_Math/tests/wscript          | 10 ++++
 2 files changed, 71 insertions(+)
 create mode 100644 libraries/AP_Math/tests/test_vectors.cpp
 create mode 100644 libraries/AP_Math/tests/wscript

diff --git a/libraries/AP_Math/tests/test_vectors.cpp b/libraries/AP_Math/tests/test_vectors.cpp
new file mode 100644
index 0000000000..94a20021be
--- /dev/null
+++ b/libraries/AP_Math/tests/test_vectors.cpp
@@ -0,0 +1,61 @@
+#include <AP_gtest.h>
+
+#include <AP_Math/AP_Math.h>
+
+#define SQRT_2 1.4142135623730951f
+
+TEST(VectorTest, Rotations)
+{
+    unsigned rotation_count = 0;
+
+#define TEST_ROTATION(rotation, x, y, z) { \
+    Vector3f v(1, 1, 1); \
+    v.rotate(rotation); \
+    EXPECT_EQ(Vector3f(x, y, z), v); \
+    rotation_count++; \
+}
+
+    TEST_ROTATION(ROTATION_NONE, 1, 1, 1);
+    TEST_ROTATION(ROTATION_YAW_45, 0, SQRT_2, 1);
+    TEST_ROTATION(ROTATION_YAW_90, -1, 1, 1);
+    TEST_ROTATION(ROTATION_YAW_135, -SQRT_2, 0, 1);
+    TEST_ROTATION(ROTATION_YAW_180, -1, -1, 1);
+    TEST_ROTATION(ROTATION_YAW_225, 0, -SQRT_2, 1);
+    TEST_ROTATION(ROTATION_YAW_270, 1, -1, 1);
+    TEST_ROTATION(ROTATION_YAW_315, SQRT_2, 0, 1);
+    TEST_ROTATION(ROTATION_ROLL_180, 1, -1, -1);
+    TEST_ROTATION(ROTATION_ROLL_180_YAW_45, SQRT_2, 0, -1);
+    TEST_ROTATION(ROTATION_ROLL_180_YAW_90, 1, 1, -1);
+    TEST_ROTATION(ROTATION_ROLL_180_YAW_135, 0, SQRT_2, -1);
+    TEST_ROTATION(ROTATION_PITCH_180, -1, 1, -1);
+    TEST_ROTATION(ROTATION_ROLL_180_YAW_225, -SQRT_2, 0, -1);
+    TEST_ROTATION(ROTATION_ROLL_180_YAW_270, -1, -1, -1);
+    TEST_ROTATION(ROTATION_ROLL_180_YAW_315, 0, -SQRT_2, -1);
+    TEST_ROTATION(ROTATION_ROLL_90, 1, -1, 1);
+    TEST_ROTATION(ROTATION_ROLL_90_YAW_45, SQRT_2, 0, 1);
+    TEST_ROTATION(ROTATION_ROLL_90_YAW_90, 1, 1, 1);
+    TEST_ROTATION(ROTATION_ROLL_90_YAW_135, 0, SQRT_2, 1);
+    TEST_ROTATION(ROTATION_ROLL_270, 1, 1, -1);
+    TEST_ROTATION(ROTATION_ROLL_270_YAW_45, 0, SQRT_2, -1);
+    TEST_ROTATION(ROTATION_ROLL_270_YAW_90, -1, 1, -1);
+    TEST_ROTATION(ROTATION_ROLL_270_YAW_135, -SQRT_2, 0, -1);
+    TEST_ROTATION(ROTATION_PITCH_90, 1, 1, -1);
+    TEST_ROTATION(ROTATION_PITCH_270, -1, 1, 1);
+    TEST_ROTATION(ROTATION_PITCH_180_YAW_90, -1, -1, -1);
+    TEST_ROTATION(ROTATION_PITCH_180_YAW_270, 1, 1, -1);
+    TEST_ROTATION(ROTATION_ROLL_90_PITCH_90, 1, -1, -1);
+    TEST_ROTATION(ROTATION_ROLL_180_PITCH_90, -1, -1, -1);
+    TEST_ROTATION(ROTATION_ROLL_270_PITCH_90, -1, 1, -1);
+    TEST_ROTATION(ROTATION_ROLL_90_PITCH_180, -1, -1, -1);
+    TEST_ROTATION(ROTATION_ROLL_270_PITCH_180, -1, 1, 1);
+    TEST_ROTATION(ROTATION_ROLL_90_PITCH_270, -1, -1, 1);
+    TEST_ROTATION(ROTATION_ROLL_180_PITCH_270, 1, -1, 1);
+    TEST_ROTATION(ROTATION_ROLL_270_PITCH_270, 1, 1, 1);
+    TEST_ROTATION(ROTATION_ROLL_90_PITCH_180_YAW_90, 1, -1, -1);
+    TEST_ROTATION(ROTATION_ROLL_90_YAW_270, -1, -1, 1);
+    TEST_ROTATION(ROTATION_YAW_293_PITCH_68_ROLL_90, -0.4118548f, -1.58903555f, -0.55257726f);
+
+    EXPECT_EQ(ROTATION_MAX, rotation_count) << "All rotations are expect to be tested";
+}
+
+AP_GTEST_MAIN()
diff --git a/libraries/AP_Math/tests/wscript b/libraries/AP_Math/tests/wscript
new file mode 100644
index 0000000000..e4d42f80d0
--- /dev/null
+++ b/libraries/AP_Math/tests/wscript
@@ -0,0 +1,10 @@
+#!/usr/bin/env python
+# encoding: utf-8
+
+import ardupilotwaf
+
+def build(bld):
+    ardupilotwaf.find_tests(
+        bld,
+        use='ap',
+    )