Support more of mathlib function for easier swap in px4.

matrix/Vector3.hpp

@@ -13,6 +13,9 @@
 namespace matrix
 {
 
+template <typename Type, size_t M, size_t N>
+class Matrix;
+
 template <typename Type, size_t M>
 class Vector;
 
@@ -60,10 +63,51 @@ public:
         return c;
     }
 
-    Vector3 operator%(const Matrix31 & b) const {
+    /**
+     * Override matrix ops so Vector3 type is returned
+     */
+
+    inline Vector3 operator+(Vector3 other) const
+    {
+        return Matrix31::operator+(other);
+    }
+
+    inline Vector3 operator-(Vector3 other) const
+    {
+        return Matrix31::operator-(other);
+    }
+
+    inline Vector3 operator-() const
+    {
+        return Matrix31::operator-();
+    }
+
+    inline Vector3 operator*(Type scalar) const
+    {
+        return Matrix31::operator*(scalar);
+    }
+
+    inline Type operator*(Vector3 b) const
+    {
+        return Vector<Type, 3>::operator*(b);
+    }
+
+    inline Vector3 operator%(const Matrix31 & b) const {
         return (*this).cross(b);
     }
 
+    /**
+     * Override vector ops so Vector3 type is returned
+     */
+    inline Vector3 unit() const {
+        return Vector3(Vector<Type, 3>::unit());
+    }
+
+    inline Vector3 normalized() const {
+        return unit();
+    }
+
+
     Dcm<Type> hat() const {    // inverse to Dcm.vee() operation
         const Vector3 &v(*this);
         Dcm<Type> A;