Support more of mathlib function for easier swap in px4.

matrix/Matrix.hpp

@@ -41,6 +41,12 @@ public:
     {
     }
 
+    Matrix(const Type data_[][N]) :
+        _data()
+    {
+        memcpy(_data, data_, sizeof(_data));
+    }
+
     Matrix(const Type *data_) :
         _data()
     {
@@ -120,6 +126,20 @@ public:
         return res;
     }
 
+    Matrix<Type, M, N> edivide(const Matrix<Type, M, N> &other) const
+    {
+        Matrix<Type, M, N> res;
+        const Matrix<Type, M, N> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                res(i , j) = self(i, j)/other(i, j);
+            }
+        }
+
+        return res;
+    }
+
     Matrix<Type, M, N> operator+(const Matrix<Type, M, N> &other) const
     {
         Matrix<Type, M, N> res;
@@ -334,6 +354,11 @@ public:
         memset(_data, 0, sizeof(_data));
     }
 
+    inline void zero()
+    {
+        setZero();
+    }
+
     void setAll(Type val)
     {
         Matrix<Type, M, N> &self = *this;
@@ -360,6 +385,11 @@ public:
         }
     }
 
+    inline void identity()
+    {
+        setIdentity();
+    }
+
     inline void swapRows(size_t a, size_t b)
     {
         if (a == b) {

matrix/Quaternion.hpp

@@ -292,6 +292,20 @@ public:
         (*this) = (*this) * res;
     }
 
+    Vector3f conjugate(const Vector3f &vec) {
+        Quaternion q = *this;
+        Quaternion v(0, vec(0), vec(1), vec(2));
+        Quaternion res = q*v*q.inversed();
+        return Vector3f(res(1), res(2), res(3));
+    }
+
+    Vector3f conjugate_inversed(const Vector3f &vec) {
+        Quaternion q = *this;
+        Quaternion v(0, vec(0), vec(1), vec(2));
+        Quaternion res = q.inversed()*v*q;
+        return Vector3f(res(1), res(2), res(3));
+    }
+
     /**
      * Rotation quaternion from vector
      *
@@ -367,6 +381,30 @@ public:
 
         return vec;
     }
+
+    /**
+     * Imaginary components of quaternion
+     */
+    Vector3<Type> imag()
+    {
+        Quaternion &q = *this;
+        return Vector3<Type>(q(1), q(2), q(3));
+    }
+
+    /**
+     * XXX DEPRECATED, can use assignment or ctor
+     */
+    Quaternion from_dcm(Matrix<Type, 3, 3> dcm) {
+        return Quaternion(Dcmf(dcm));
+    }
+
+    /**
+     * XXX DEPRECATED, can use assignment or ctor
+     */
+    Dcm<Type> to_dcm() {
+        return Dcm<Type>(*this);
+    }
+
 };
 
 typedef Quaternion<float> Quatf;

matrix/Vector.hpp

@@ -61,11 +61,24 @@ public:
         return r;
     }
 
+    inline Type operator*(const MatrixM1 & b) const {
+        const Vector &a(*this);
+        return a.dot(b);
+    }
+
+    inline Vector operator*(float b) const {
+        return Vector(MatrixM1::operator*(b));
+    }
+
     Type norm() const {
         const Vector &a(*this);
         return Type(sqrt(a.dot(a)));
     }
 
+    inline Type length() const {
+        return norm();
+    }
+
     inline void normalize() {
         (*this) /= norm();
     }
@@ -74,6 +87,10 @@ public:
         return (*this) / norm();
     }
 
+    inline Vector normalized() const {
+        return unit();
+    }
+
     Vector pow(Type v) const {
         const Vector &a(*this);
         Vector r;

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;