Improved on old unit tests

src/Matrix.cpp

@@ -13,12 +13,6 @@
 #include <cmath>
 #include <cstdlib>
 #include <cstring>
-#include <type_traits>
-
-template <uint8_t rows, uint8_t columns>
-Matrix<rows, columns>::Matrix(float value) {
-  this->Fill(value);
-}
 
 template <uint8_t rows, uint8_t columns>
 Matrix<rows, columns>::Matrix(const std::array<float, rows * columns> &array) {
@@ -32,6 +26,14 @@ Matrix<rows, columns>::Matrix(Args... args) {
                                static_cast<uint16_t>(columns)};
 
   std::initializer_list<float> initList{static_cast<float>(args)...};
+  // if there is only one value, we actually want to do a fill
+  if (sizeof...(args) == 1) {
+    this->Fill(*initList.begin());
+  }
+  static_assert(sizeof...(args) == arraySize || sizeof...(args) == 1,
+                "You did not provide the right amount of initializers for this "
+                "matrix size");
+
   // choose whichever buffer size is smaller for the copy length
   uint32_t minSize =
       std::min(arraySize, static_cast<uint16_t>(initList.size()));
@@ -39,11 +41,13 @@ Matrix<rows, columns>::Matrix(Args... args) {
 }
 
 template <uint8_t rows, uint8_t columns>
-void Matrix<rows, columns>::Identity() {
-  this->Fill(0);
-  for (uint8_t idx{0}; idx < rows; idx++) {
-    this->matrix[idx * columns + idx] = 1;
+Matrix<rows, columns> Matrix<rows, columns>::Identity() {
+  Matrix<rows, columns> identityMatrix{0};
+  uint32_t minDimension = std::min(rows, columns);
+  for (uint8_t idx{0}; idx < minDimension; idx++) {
+    identityMatrix[idx][idx] = 1;
   }
+  return identityMatrix;
 }
 
 template <uint8_t rows, uint8_t columns>
@@ -564,16 +568,18 @@ void Matrix<rows, columns>::EigenQR(Matrix<rows, rows> &eigenVectors,
                                     uint32_t maxIterations,
                                     float tolerance) const {
   static_assert(rows > 1, "Matrix size must be > 1 for QR iteration");
+  static_assert(rows == columns, "Matrix size must be square for QR iteration");
 
   Matrix<rows, rows> Ak = *this; // Copy original matrix
-  Matrix<rows, rows> QQ{};
-  QQ.Identity();
+  Matrix<rows, rows> QQ{Matrix<rows, rows>::Identity()};
 
   for (uint32_t iter = 0; iter < maxIterations; ++iter) {
-    Matrix<rows, rows> Q, R;
-    Ak.QRDecomposition(Q, R);
+    Matrix<rows, rows> Q, R, shift;
 
-    Ak = R * Q;
+    // QR shift lets us "attack" the first diagonal to speed up the algorithm
+    shift = Matrix<rows, rows>::Identity() * Ak[rows - 1][rows - 1];
+    (Ak - shift).QRDecomposition(Q, R);
+    Ak = R * Q + shift;
     QQ = QQ * Q;
 
     // Check convergence: off-diagonal norm

src/Matrix.hpp

@@ -18,11 +18,6 @@ public:
    */
   Matrix() = default;
 
-  /**
-   * @brief Create a matrix but fill all of its entries with one value
-   */
-  Matrix(float value);
-
   /**
    * @brief Initialize a matrix with an array
    */
@@ -39,9 +34,9 @@ public:
   template <typename... Args> Matrix(Args... args);
 
   /**
-   * @brief set the matrix diagonals to 1 and all other values to 0
+   * @brief Create an identity matrix
    */
-  void Identity();
+  static Matrix<rows, columns> Identity();
 
   /**
    * @brief Set all elements in this to value

src/Quaternion.h

@@ -2,90 +2,89 @@
 #define QUATERNION_H_
 
 #include "Matrix.hpp"
-class Quaternion : public Matrix<1, 4>
-{
+class Quaternion : public Matrix<1, 4> {
 public:
-    Quaternion() : Matrix<1, 4>() {}
-    Quaternion(float fillValue) : Matrix<1, 4>(fillValue) {}
-    Quaternion(float w, float v1, float v2, float v3) : Matrix<1, 4>(w, v1, v2, v3) {}
-    Quaternion(const Quaternion &q) : Matrix<1, 4>(q.w, q.v1, q.v2, q.v3) {}
-    Quaternion(const Matrix<1, 4> &matrix) : Matrix<1, 4>(matrix) {}
-    Quaternion(const std::array<float, 4> &array) : Matrix<1, 4>(array) {}
+  Quaternion() : Matrix<1, 4>() {}
+  Quaternion(float w, float v1, float v2, float v3)
+      : Matrix<1, 4>(w, v1, v2, v3) {}
+  Quaternion(const Quaternion &q) : Matrix<1, 4>(q.w, q.v1, q.v2, q.v3) {}
+  Quaternion(const Matrix<1, 4> &matrix) : Matrix<1, 4>(matrix) {}
+  Quaternion(const std::array<float, 4> &array) : Matrix<1, 4>(array) {}
 
-    /**
-     * @brief Create a quaternion from an angle and axis
-     * @param angle The angle to rotate by
-     * @param axis The axis to rotate around
-     */
-    static Quaternion FromAngleAndAxis(float angle, const Matrix<1, 3> &axis);
+  /**
+   * @brief Create a quaternion from an angle and axis
+   * @param angle The angle to rotate by
+   * @param axis The axis to rotate around
+   */
+  static Quaternion FromAngleAndAxis(float angle, const Matrix<1, 3> &axis);
 
-    /**
-     * @brief Access the elements of the quaternion
-     * @param index The index of the element to access
-     * @return The value of the element at the index
-     */
-    float operator[](uint8_t index) const;
+  /**
+   * @brief Access the elements of the quaternion
+   * @param index The index of the element to access
+   * @return The value of the element at the index
+   */
+  float operator[](uint8_t index) const;
 
-    /**
-     * @brief Assign one quaternion to another
-     */
-    void operator=(const Quaternion &other);
+  /**
+   * @brief Assign one quaternion to another
+   */
+  void operator=(const Quaternion &other);
 
-    /**
-     * @brief Do quaternion multiplication
-     */
-    Quaternion operator*(const Quaternion &other) const;
+  /**
+   * @brief Do quaternion multiplication
+   */
+  Quaternion operator*(const Quaternion &other) const;
 
-    /**
-     * @brief Multiply the quaternion by a scalar
-     */
-    Quaternion operator*(float scalar) const;
+  /**
+   * @brief Multiply the quaternion by a scalar
+   */
+  Quaternion operator*(float scalar) const;
 
-    /**
-     * @brief Add two quaternions together
-     * @param other The quaternion to add to this one
-     * @return The net quaternion
-     */
-    Quaternion operator+(const Quaternion &other) const;
+  /**
+   * @brief Add two quaternions together
+   * @param other The quaternion to add to this one
+   * @return The net quaternion
+   */
+  Quaternion operator+(const Quaternion &other) const;
 
-    /**
-     * @brief Q_Mult a quaternion by another quaternion
-     * @param other The quaternion to rotate by
-     * @param buffer The buffer to store the result in
-     * @return A reference to the buffer
-     */
-    Quaternion &Q_Mult(const Quaternion &other, Quaternion &buffer) const;
+  /**
+   * @brief Q_Mult a quaternion by another quaternion
+   * @param other The quaternion to rotate by
+   * @param buffer The buffer to store the result in
+   * @return A reference to the buffer
+   */
+  Quaternion &Q_Mult(const Quaternion &other, Quaternion &buffer) const;
 
-    /**
-     * @brief Rotate a quaternion by this quaternion
-     * @param other The quaternion to rotate
-     * @param buffer The buffer to store the result in
-     *
-     */
-    Quaternion &Rotate(Quaternion &other, Quaternion &buffer) const;
+  /**
+   * @brief Rotate a quaternion by this quaternion
+   * @param other The quaternion to rotate
+   * @param buffer The buffer to store the result in
+   *
+   */
+  Quaternion &Rotate(Quaternion &other, Quaternion &buffer) const;
 
-    /**
-     * @brief Normalize the quaternion to a magnitude of 1
-     */
-    void Normalize();
+  /**
+   * @brief Normalize the quaternion to a magnitude of 1
+   */
+  void Normalize();
 
-    /**
-     * @brief Convert the quaternion to a rotation matrix
-     * @return The rotation matrix
-     */
-    Matrix<3, 3> ToRotationMatrix() const;
+  /**
+   * @brief Convert the quaternion to a rotation matrix
+   * @return The rotation matrix
+   */
+  Matrix<3, 3> ToRotationMatrix() const;
 
-    /**
-     * @brief Convert the quaternion to an Euler angle representation
-     * @return The Euler angle representation of the quaternion
-     */
-    Matrix<3, 1> ToEulerAngle() const;
+  /**
+   * @brief Convert the quaternion to an Euler angle representation
+   * @return The Euler angle representation of the quaternion
+   */
+  Matrix<3, 1> ToEulerAngle() const;
 
-    // Give people an easy way to access the elements
-    float &w{matrix[0]};
-    float &v1{matrix[1]};
-    float &v2{matrix[2]};
-    float &v3{matrix[3]};
+  // Give people an easy way to access the elements
+  float &w{matrix[0]};
+  float &v1{matrix[1]};
+  float &v2{matrix[2]};
+  float &v3{matrix[3]};
 };
 
 #endif // QUATERNION_H_