Replaced normalize with EuclideanNorm

src/Matrix.cpp

@@ -277,6 +277,11 @@ void Matrix<rows, columns>::ToString(std::string &stringBuffer) const {
   }
 }
 
+template <uint8_t rows, uint8_t columns>
+const float *Matrix<rows, columns>::ToArray() const {
+  return this->matrix.data();
+}
+
 template <uint8_t rows, uint8_t columns>
 std::array<float, columns> &
 Matrix<rows, columns>::operator[](uint8_t row_index) {
@@ -418,8 +423,8 @@ Matrix<rows, columns>::adjugate(Matrix<rows, columns> &result) const {
 }
 
 template <uint8_t rows, uint8_t columns>
-Matrix<rows, columns> &
+Matrix<rows, columns> Matrix<rows, columns>::EuclideanNorm() const {
-Matrix<rows, columns>::Normalize(Matrix<rows, columns> &result) const {
+
   float sum{0};
   for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
     for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
@@ -428,14 +433,14 @@ Matrix<rows, columns>::Normalize(Matrix<rows, columns> &result) const {
     }
   }
 
+  Matrix<rows, columns> result{};
   if (sum == 0) {
     // this wouldn't do anything anyways
-    result.Fill(1e+6);
+    result.Fill(0);
     return result;
   }
 
   sum = sqrt(sum);
-
   for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
     for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
       result[row_idx][column_idx] = this->Get(row_idx, column_idx) / sum;
@@ -491,11 +496,9 @@ void Matrix<rows, columns>::SetSubMatrix(
 template <uint8_t rows, uint8_t columns>
 void Matrix<rows, columns>::QRDecomposition(Matrix<rows, columns> &Q,
                                             Matrix<columns, columns> &R) const {
-
   static_assert(columns <= rows, "QR decomposition requires columns <= rows");
-  // Gram-Schmidt orthogonalization
+
-  Matrix<rows, 1> a_col, u, e, proj;
+  Matrix<rows, 1> a_col, u, q_col, proj;
-  Matrix<rows, 1> q_col;
   Q.Fill(0);
   R.Fill(0);
 
@@ -505,18 +508,17 @@ void Matrix<rows, columns>::QRDecomposition(Matrix<rows, columns> &Q,
 
     for (uint8_t j = 0; j < k; ++j) {
       Q.GetColumn(j, q_col);
-      float r_jk = Matrix<rows, 1>::DotProduct(q_col, a_col);
+      float r_jk = Matrix<rows, 1>::DotProduct(
+          q_col, u); // FIXED: use u instead of a_col
       R[j][k] = r_jk;
 
-      // proj = r_jk * q_j
       proj = q_col * r_jk;
       u = u - proj;
     }
 
     float norm = sqrt(Matrix<rows, 1>::DotProduct(u, u));
-    if (norm == 0) {
+    if (norm < 1e-12f)
-      norm = 1e-12f; // avoid div by zero
+      norm = 1e-12f; // for stability
-    }
 
     for (uint8_t i = 0; i < rows; ++i) {
       Q[i][k] = u[i][0] / norm;

src/Matrix.hpp

@@ -132,7 +132,7 @@ public:
    * @brief reduce the matrix so the sum of its elements equal 1
    * @param result a buffer to store the result into
    */
-  Matrix<rows, columns> &Normalize(Matrix<rows, columns> &result) const;
+  Matrix<rows, columns> EuclideanNorm() const;
 
   /**
    * @brief Get a row from the matrix
@@ -159,8 +159,16 @@ public:
    */
   constexpr uint8_t GetColumnSize() { return columns; }
 
+  /**
+   * @brief Write a string representation of the matrix into the buffer
+   */
   void ToString(std::string &stringBuffer) const;
 
+  /**
+   * @brief Returns the internal representation of the matrix as an array
+   */
+  const float *ToArray() const;
+
   /**
    * @brief Get an element from the matrix
    * @param row the row index of the element

src/Quaternion.cpp

@@ -6,115 +6,116 @@
  * @param angle The angle to rotate by
  * @param axis The axis to rotate around
  */
-Quaternion Quaternion::FromAngleAndAxis(float angle, const Matrix<1, 3> &axis)
+Quaternion Quaternion::FromAngleAndAxis(float angle, const Matrix<1, 3> &axis) {
-{
+  const float halfAngle = angle / 2;
-    const float halfAngle = angle / 2;
+  const float sinHalfAngle = sin(halfAngle);
-    const float sinHalfAngle = sin(halfAngle);
+  Matrix<1, 3> normalizedAxis{};
-    Matrix<1, 3> normalizedAxis{};
+  normalizedAxis = axis.EuclideanNorm();
-    axis.Normalize(normalizedAxis);
+  return Quaternion{static_cast<float>(cos(halfAngle)),
-    return Quaternion{
+                    normalizedAxis.Get(0, 0) * sinHalfAngle,
-        static_cast<float>(cos(halfAngle)),
+                    normalizedAxis.Get(0, 1) * sinHalfAngle,
-        normalizedAxis.Get(0, 0) * sinHalfAngle,
+                    normalizedAxis.Get(0, 2) * sinHalfAngle};
-        normalizedAxis.Get(0, 1) * sinHalfAngle,
-        normalizedAxis.Get(0, 2) * sinHalfAngle};
 }
 
-float Quaternion::operator[](uint8_t index) const
+float Quaternion::operator[](uint8_t index) const {
-{
+  if (index < 4) {
-    if (index < 4)
+    return this->matrix[index];
-    {
+  }
-        return this->matrix[index];
-    }
 
-    // index out of bounds
+  // index out of bounds
-    return 1e+6;
+  return 1e+6;
 }
 
-void Quaternion::operator=(const Quaternion &other)
+void Quaternion::operator=(const Quaternion &other) {
-{
+  memcpy(&(this->matrix), &(other.matrix), 4 * sizeof(float));
-    memcpy(&(this->matrix), &(other.matrix), 4 * sizeof(float));
 }
 
-Quaternion Quaternion::operator*(const Quaternion &other) const
+Quaternion Quaternion::operator*(const Quaternion &other) const {
-{
+  Quaternion result{};
-    Quaternion result{};
+  this->Q_Mult(other, result);
-    this->Q_Mult(other, result);
+  return result;
-    return result;
 }
 
-Quaternion Quaternion::operator*(float scalar) const
+Quaternion Quaternion::operator*(float scalar) const {
-{
+  return Quaternion{this->w * scalar, this->v1 * scalar, this->v2 * scalar,
-    return Quaternion{this->w * scalar, this->v1 * scalar, this->v2 * scalar, this->v3 * scalar};
+                    this->v3 * scalar};
 }
 
-Quaternion Quaternion::operator+(const Quaternion &other) const
+Quaternion Quaternion::operator+(const Quaternion &other) const {
-{
+  return Quaternion{this->w + other.w, this->v1 + other.v1, this->v2 + other.v2,
-    return Quaternion{this->w + other.w, this->v1 + other.v1, this->v2 + other.v2, this->v3 + other.v3};
+                    this->v3 + other.v3};
 }
 
-Quaternion &
+Quaternion &Quaternion::Q_Mult(const Quaternion &other,
-Quaternion::Q_Mult(const Quaternion &other, Quaternion &buffer) const
+                               Quaternion &buffer) const {
-{
 
-    // eq. 6
+  // eq. 6
-    buffer.w = (other.w * this->w - other.v1 * this->v1 - other.v2 * this->v2 - other.v3 * this->v3);
+  buffer.w = (other.w * this->w - other.v1 * this->v1 - other.v2 * this->v2 -
-    buffer.v1 = (other.w * this->v1 + other.v1 * this->w - other.v2 * this->v3 + other.v3 * this->v2);
+              other.v3 * this->v3);
-    buffer.v2 = (other.w * this->v2 + other.v1 * this->v3 + other.v2 * this->w - other.v3 * this->v1);
+  buffer.v1 = (other.w * this->v1 + other.v1 * this->w - other.v2 * this->v3 +
-    buffer.v3 = (other.w * this->v3 - other.v1 * this->v2 + other.v2 * this->v1 + other.v3 * this->w);
+               other.v3 * this->v2);
-    return buffer;
+  buffer.v2 = (other.w * this->v2 + other.v1 * this->v3 + other.v2 * this->w -
+               other.v3 * this->v1);
+  buffer.v3 = (other.w * this->v3 - other.v1 * this->v2 + other.v2 * this->v1 +
+               other.v3 * this->w);
+  return buffer;
 }
 
-Quaternion &Quaternion::Rotate(Quaternion &other, Quaternion &buffer) const
+Quaternion &Quaternion::Rotate(Quaternion &other, Quaternion &buffer) const {
-{
+  Quaternion prime{this->w, -this->v1, -this->v2, -this->v3};
-    Quaternion prime{this->w, -this->v1, -this->v2, -this->v3};
+  buffer.v1 = other.v1;
-    buffer.v1 = other.v1;
+  buffer.v2 = other.v2;
-    buffer.v2 = other.v2;
+  buffer.v3 = other.v3;
-    buffer.v3 = other.v3;
+  buffer.w = 0;
-    buffer.w = 0;
 
-    Quaternion temp{};
+  Quaternion temp{};
-    this->Q_Mult(buffer, temp);
+  this->Q_Mult(buffer, temp);
-    temp.Q_Mult(prime, buffer);
+  temp.Q_Mult(prime, buffer);
-    return buffer;
+  return buffer;
 }
 
-void Quaternion::Normalize()
+void Quaternion::Normalize() {
-{
+  float magnitude = sqrt(this->v1 * this->v1 + this->v2 * this->v2 +
-    float magnitude = sqrt(this->v1 * this->v1 + this->v2 * this->v2 + this->v3 * this->v3 + this->w * this->w);
+                         this->v3 * this->v3 + this->w * this->w);
-    if (magnitude == 0)
+  if (magnitude == 0) {
-    {
+    return;
-        return;
+  }
-    }
+  this->v1 /= magnitude;
-    this->v1 /= magnitude;
+  this->v2 /= magnitude;
-    this->v2 /= magnitude;
+  this->v3 /= magnitude;
-    this->v3 /= magnitude;
+  this->w /= magnitude;
-    this->w /= magnitude;
 }
 
-Matrix<3, 3> Quaternion::ToRotationMatrix() const
+Matrix<3, 3> Quaternion::ToRotationMatrix() const {
-{
+  float xx = this->v1 * this->v1;
-    float xx = this->v1 * this->v1;
+  float yy = this->v2 * this->v2;
-    float yy = this->v2 * this->v2;
+  float zz = this->v3 * this->v3;
-    float zz = this->v3 * this->v3;
+  Matrix<3, 3> rotationMatrix{1 - 2 * (yy - zz),
-    Matrix<3, 3> rotationMatrix{
+                              2 * (this->v1 * this->v2 - this->v3 * this->w),
-        1 - 2 * (yy - zz), 2 * (this->v1 * this->v2 - this->v3 * this->w), 2 * (this->v1 * this->v3 + this->v2 * this->w),
+                              2 * (this->v1 * this->v3 + this->v2 * this->w),
-        2 * (this->v1 * this->v2 + this->v3 * this->w), 1 - 2 * (xx - zz), 2 * (this->v2 * this->v3 - this->v1 * this->w),
+                              2 * (this->v1 * this->v2 + this->v3 * this->w),
-        2 * (this->v1 * this->v3 - this->v2 * this->w), 2 * (this->v2 * this->v3 + this->v1 * this->w), 1 - 2 * (xx - yy)};
+                              1 - 2 * (xx - zz),
-    return rotationMatrix;
+                              2 * (this->v2 * this->v3 - this->v1 * this->w),
+                              2 * (this->v1 * this->v3 - this->v2 * this->w),
+                              2 * (this->v2 * this->v3 + this->v1 * this->w),
+                              1 - 2 * (xx - yy)};
+  return rotationMatrix;
 };
 
-Matrix<3, 1> Quaternion::ToEulerAngle() const
+Matrix<3, 1> Quaternion::ToEulerAngle() const {
-{
+  float sqv1 = this->v1 * this->v1;
-    float sqv1 = this->v1 * this->v1;
+  float sqv2 = this->v2 * this->v2;
-    float sqv2 = this->v2 * this->v2;
+  float sqv3 = this->v3 * this->v3;
-    float sqv3 = this->v3 * this->v3;
+  float sqw = this->w * this->w;
-    float sqw = this->w * this->w;
 
-    Matrix<3, 1> eulerAngle;
+  Matrix<3, 1> eulerAngle;
-    {
+  {
-        atan2(2.0 * (this->v1 * this->v2 + this->v3 * this->w), (sqv1 - sqv2 - sqv3 + sqw));
+    atan2(2.0 * (this->v1 * this->v2 + this->v3 * this->w),
-        asin(-2.0 * (this->v1 * this->v3 - this->v2 * this->w) / (sqv1 + sqv2 + sqv3 + sqw));
+          (sqv1 - sqv2 - sqv3 + sqw));
-        atan2(2.0 * (this->v2 * this->v3 + this->v1 * this->w), (-sqv1 - sqv2 + sqv3 + sqw));
+    asin(-2.0 * (this->v1 * this->v3 - this->v2 * this->w) /
-    };
+         (sqv1 + sqv2 + sqv3 + sqw));
-    return eulerAngle;
+    atan2(2.0 * (this->v2 * this->v3 + this->v1 * this->w),
+          (-sqv1 - sqv2 + sqv3 + sqw));
+  };
+  return eulerAngle;
 }

unit-tests/matrix-tests.cpp

@@ -282,26 +282,6 @@ TEST_CASE("Elementary Matrix Operations", "Matrix") {
     REQUIRE(mat5.Get(2, 1) == 6);
   }
 
-  SECTION("Normalize") {
-    mat1.Normalize(mat3);
-
-    float sqrt_30{static_cast<float>(sqrt(30.0f))};
-
-    REQUIRE(mat3.Get(0, 0) == 1 / sqrt_30);
-    REQUIRE(mat3.Get(0, 1) == 2 / sqrt_30);
-    REQUIRE(mat3.Get(1, 0) == 3 / sqrt_30);
-    REQUIRE(mat3.Get(1, 1) == 4 / sqrt_30);
-
-    Matrix<2, 1> mat4{-0.878877044, 2.92092276};
-    Matrix<2, 1> mat5{};
-    mat4.Normalize(mat5);
-
-    REQUIRE_THAT(mat5.Get(0, 0),
-                 Catch::Matchers::WithinRel(-0.288129855179f, 1e-6f));
-    REQUIRE_THAT(mat5.Get(1, 0),
-                 Catch::Matchers::WithinRel(0.957591346325f, 1e-6f));
-  }
-
   SECTION("GET ROW") {
     Matrix<1, 2> mat1Rows{};
     mat1.GetRow(0, mat1Rows);
@@ -383,6 +363,63 @@ TEST_CASE("Elementary Matrix Operations", "Matrix") {
   }
 }
 
+template <uint8_t rows, uint8_t columns>
+float matrixSum(const Matrix<rows, columns> &matrix) {
+  float sum = 0;
+  for (uint32_t i = 0; i < rows * columns; i++) {
+    float number = matrix.ToArray()[i];
+    sum += number * number;
+  }
+  return std::sqrt(sum);
+}
+
+TEST_CASE("Normalization", "Matrix") {
+
+  SECTION("2x2 Normalize") {
+    Matrix<2, 2> mat1{1, 2, 3, 4};
+    Matrix<2, 2> mat2{};
+
+    mat2 = mat1.EuclideanNorm();
+
+    float sqrt_30{static_cast<float>(sqrt(30.0f))};
+
+    REQUIRE(mat2.Get(0, 0) == 1 / sqrt_30);
+    REQUIRE(mat2.Get(0, 1) == 2 / sqrt_30);
+    REQUIRE(mat2.Get(1, 0) == 3 / sqrt_30);
+    REQUIRE(mat2.Get(1, 1) == 4 / sqrt_30);
+
+    REQUIRE_THAT(matrixSum(mat2), Catch::Matchers::WithinRel(1.0f, 1e-6f));
+  }
+
+  SECTION("2x1 (Vector) Normalize") {
+    Matrix<2, 1> mat1{-0.878877044, 2.92092276};
+    Matrix<2, 1> mat2{};
+    mat2 = mat1.EuclideanNorm();
+
+    REQUIRE_THAT(mat2.Get(0, 0),
+                 Catch::Matchers::WithinRel(-0.288129855179f, 1e-6f));
+    REQUIRE_THAT(mat2.Get(1, 0),
+                 Catch::Matchers::WithinRel(0.957591346325f, 1e-6f));
+
+    float sum = matrixSum(mat2);
+    REQUIRE_THAT(sum, Catch::Matchers::WithinRel(1.0f, 1e-6f));
+  }
+
+  SECTION("Normalized vectors sum to 1") {
+    Matrix<9, 1> mat1{1, 2, 3, 4, 5, 6, 7, 8, 9};
+    Matrix<9, 1> mat2;
+    mat2 = mat1.EuclideanNorm();
+    float sum = matrixSum(mat2);
+    REQUIRE_THAT(sum, Catch::Matchers::WithinRel(1.0f, 1e-6f));
+
+    Matrix<2, 3> mat3{1, 2, 3, 4, 5, 6};
+    Matrix<2, 3> mat4{};
+    mat4 = mat3.EuclideanNorm();
+    sum = matrixSum(mat4);
+    REQUIRE_THAT(sum, Catch::Matchers::WithinRel(1.0f, 1e-6f));
+  }
+}
+
 TEST_CASE("QR Decompositions", "Matrix") {
   SECTION("2x2 QRDecomposition") {
     Matrix<2, 2> A{1.0f, 2.0f, 3.0f, 4.0f};
@@ -434,6 +471,13 @@ TEST_CASE("QR Decompositions", "Matrix") {
     Matrix<3, 3> Q{}, R{};
     A.QRDecomposition(Q, R);
 
+    std::string strBuf1 = "";
+    Q.ToString(strBuf1);
+    std::cout << "Q:\n" << strBuf1 << std::endl;
+    strBuf1 = "";
+    R.ToString(strBuf1);
+    std::cout << "R:\n" << strBuf1 << std::endl;
+
     // Check that Q * R ≈ A
     Matrix<3, 3> QR{};
     QR = Q * R;
@@ -463,13 +507,6 @@ TEST_CASE("QR Decompositions", "Matrix") {
       }
     }
 
-    std::string strBuf1 = "";
-    Q.ToString(strBuf1);
-    std::cout << "Q:\n" << strBuf1 << std::endl;
-    strBuf1 = "";
-    R.ToString(strBuf1);
-    std::cout << "R:\n" << strBuf1 << std::endl;
-
     // check that all Q values are correct
     REQUIRE_THAT(Q[0][0], Catch::Matchers::WithinRel(0.1231f, 1e-4f));
     REQUIRE_THAT(Q[0][1], Catch::Matchers::WithinRel(0.904534f, 1e-4f));

unit-tests/matrix-timing-tests.cpp

@@ -99,7 +99,7 @@ TEST_CASE("Timing Tests", "Matrix") {
 
   SECTION("Normalize") {
     for (uint32_t i{0}; i < 10000; i++) {
-      mat1.Normalize(mat3);
+      mat3 = mat1.EuclideanNorm();
     }
   }