From d4eaf09baf81a225ffce4e08ba466e1d91144470 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 1 Oct 2018 13:04:11 +1000
Subject: [PATCH] AP_Math: added rotation_equal()

---
 libraries/AP_Math/AP_Math.cpp | 18 ++++++++++++++++++
 libraries/AP_Math/AP_Math.h   |  2 ++
 2 files changed, 20 insertions(+)

diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp
index 44eec76e2d..0fe5bc3e82 100644
--- a/libraries/AP_Math/AP_Math.cpp
+++ b/libraries/AP_Math/AP_Math.cpp
@@ -255,3 +255,21 @@ bool is_valid_octal(uint16_t octal)
     }
     return true;
 }
+
+/*
+  return true if two rotations are equivalent
+  This copes with the fact that we have some duplicates, like ROLL_180_YAW_90 and PITCH_180_YAW_270
+ */
+bool rotation_equal(enum Rotation r1, enum Rotation r2)
+{
+    if (r1 == r2) {
+        return true;
+    }
+    Vector3f v(1,2,3);
+    Vector3f v1 = v;
+    Vector3f v2 = v;
+    v1.rotate(r1);
+    v2.rotate(r2);
+    return (v1 - v2).length() < 0.001;
+}
+
diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h
index 3ca6a87d61..8484dbf090 100644
--- a/libraries/AP_Math/AP_Math.h
+++ b/libraries/AP_Math/AP_Math.h
@@ -258,3 +258,5 @@ Vector3f rand_vec3f(void);
 // confirm a value is a valid octal value
 bool is_valid_octal(uint16_t octal);
 
+// return true if two rotations are equal
+bool rotation_equal(enum Rotation r1, enum Rotation r2);