diff --git a/src/Matrix.cpp b/src/Matrix.cpp index e1961a2..570ee84 100644 --- a/src/Matrix.cpp +++ b/src/Matrix.cpp @@ -277,6 +277,11 @@ void Matrix::ToString(std::string &stringBuffer) const { } } +template +const float *Matrix::ToArray() const { + return this->matrix.data(); +} + template std::array & Matrix::operator[](uint8_t row_index) { @@ -418,8 +423,8 @@ Matrix::adjugate(Matrix &result) const { } template -Matrix & -Matrix::Normalize(Matrix &result) const { +Matrix Matrix::EuclideanNorm() 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::Normalize(Matrix &result) const { } } + Matrix 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::SetSubMatrix( template void Matrix::QRDecomposition(Matrix &Q, Matrix &R) const { - static_assert(columns <= rows, "QR decomposition requires columns <= rows"); - // Gram-Schmidt orthogonalization - Matrix a_col, u, e, proj; - Matrix q_col; + + Matrix a_col, u, q_col, proj; Q.Fill(0); R.Fill(0); @@ -505,18 +508,17 @@ void Matrix::QRDecomposition(Matrix &Q, for (uint8_t j = 0; j < k; ++j) { Q.GetColumn(j, q_col); - float r_jk = Matrix::DotProduct(q_col, a_col); + float r_jk = Matrix::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::DotProduct(u, u)); - if (norm == 0) { - norm = 1e-12f; // avoid div by zero - } + if (norm < 1e-12f) + norm = 1e-12f; // for stability for (uint8_t i = 0; i < rows; ++i) { Q[i][k] = u[i][0] / norm; diff --git a/src/Matrix.hpp b/src/Matrix.hpp index 66f958d..b7ea1b9 100644 --- a/src/Matrix.hpp +++ b/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 &Normalize(Matrix &result) const; + Matrix 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 diff --git a/src/Quaternion.cpp b/src/Quaternion.cpp index fceb948..204896d 100644 --- a/src/Quaternion.cpp +++ b/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) -{ - const float halfAngle = angle / 2; - const float sinHalfAngle = sin(halfAngle); - Matrix<1, 3> normalizedAxis{}; - axis.Normalize(normalizedAxis); - return Quaternion{ - static_cast(cos(halfAngle)), - normalizedAxis.Get(0, 0) * sinHalfAngle, - normalizedAxis.Get(0, 1) * sinHalfAngle, - normalizedAxis.Get(0, 2) * sinHalfAngle}; +Quaternion Quaternion::FromAngleAndAxis(float angle, const Matrix<1, 3> &axis) { + const float halfAngle = angle / 2; + const float sinHalfAngle = sin(halfAngle); + Matrix<1, 3> normalizedAxis{}; + normalizedAxis = axis.EuclideanNorm(); + return Quaternion{static_cast(cos(halfAngle)), + normalizedAxis.Get(0, 0) * sinHalfAngle, + normalizedAxis.Get(0, 1) * sinHalfAngle, + normalizedAxis.Get(0, 2) * sinHalfAngle}; } -float Quaternion::operator[](uint8_t index) const -{ - if (index < 4) - { - return this->matrix[index]; - } +float Quaternion::operator[](uint8_t index) const { + if (index < 4) { + return this->matrix[index]; + } - // index out of bounds - return 1e+6; + // index out of bounds + return 1e+6; } -void Quaternion::operator=(const Quaternion &other) -{ - memcpy(&(this->matrix), &(other.matrix), 4 * sizeof(float)); +void Quaternion::operator=(const Quaternion &other) { + memcpy(&(this->matrix), &(other.matrix), 4 * sizeof(float)); } -Quaternion Quaternion::operator*(const Quaternion &other) const -{ - Quaternion result{}; - this->Q_Mult(other, result); - return result; +Quaternion Quaternion::operator*(const Quaternion &other) const { + Quaternion result{}; + this->Q_Mult(other, result); + return result; } -Quaternion Quaternion::operator*(float scalar) const -{ - return Quaternion{this->w * scalar, this->v1 * scalar, this->v2 * scalar, this->v3 * scalar}; +Quaternion Quaternion::operator*(float scalar) const { + return Quaternion{this->w * scalar, this->v1 * scalar, this->v2 * scalar, + this->v3 * scalar}; } -Quaternion Quaternion::operator+(const Quaternion &other) const -{ - return Quaternion{this->w + other.w, this->v1 + other.v1, this->v2 + other.v2, this->v3 + other.v3}; +Quaternion Quaternion::operator+(const Quaternion &other) const { + return Quaternion{this->w + other.w, this->v1 + other.v1, this->v2 + other.v2, + this->v3 + other.v3}; } -Quaternion & -Quaternion::Q_Mult(const Quaternion &other, Quaternion &buffer) const -{ +Quaternion &Quaternion::Q_Mult(const Quaternion &other, + Quaternion &buffer) const { - // eq. 6 - buffer.w = (other.w * this->w - other.v1 * this->v1 - other.v2 * this->v2 - other.v3 * this->v3); - buffer.v1 = (other.w * this->v1 + other.v1 * this->w - other.v2 * this->v3 + other.v3 * this->v2); - 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; + // eq. 6 + buffer.w = (other.w * this->w - other.v1 * this->v1 - other.v2 * this->v2 - + other.v3 * this->v3); + buffer.v1 = (other.w * this->v1 + other.v1 * this->w - other.v2 * this->v3 + + other.v3 * this->v2); + 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 prime{this->w, -this->v1, -this->v2, -this->v3}; - buffer.v1 = other.v1; - buffer.v2 = other.v2; - buffer.v3 = other.v3; - buffer.w = 0; +Quaternion &Quaternion::Rotate(Quaternion &other, Quaternion &buffer) const { + Quaternion prime{this->w, -this->v1, -this->v2, -this->v3}; + buffer.v1 = other.v1; + buffer.v2 = other.v2; + buffer.v3 = other.v3; + buffer.w = 0; - Quaternion temp{}; - this->Q_Mult(buffer, temp); - temp.Q_Mult(prime, buffer); - return buffer; + Quaternion temp{}; + this->Q_Mult(buffer, temp); + temp.Q_Mult(prime, buffer); + return buffer; } -void Quaternion::Normalize() -{ - float magnitude = sqrt(this->v1 * this->v1 + this->v2 * this->v2 + this->v3 * this->v3 + this->w * this->w); - if (magnitude == 0) - { - return; - } - this->v1 /= magnitude; - this->v2 /= magnitude; - this->v3 /= magnitude; - this->w /= magnitude; +void Quaternion::Normalize() { + float magnitude = sqrt(this->v1 * this->v1 + this->v2 * this->v2 + + this->v3 * this->v3 + this->w * this->w); + if (magnitude == 0) { + return; + } + this->v1 /= magnitude; + this->v2 /= magnitude; + this->v3 /= magnitude; + this->w /= magnitude; } -Matrix<3, 3> Quaternion::ToRotationMatrix() const -{ - float xx = this->v1 * this->v1; - float yy = this->v2 * this->v2; - float zz = this->v3 * this->v3; - Matrix<3, 3> rotationMatrix{ - 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->v2 + this->v3 * this->w), 1 - 2 * (xx - zz), 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, 3> Quaternion::ToRotationMatrix() const { + float xx = this->v1 * this->v1; + float yy = this->v2 * this->v2; + float zz = this->v3 * this->v3; + Matrix<3, 3> rotationMatrix{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->v2 + this->v3 * this->w), + 1 - 2 * (xx - zz), + 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 -{ - float sqv1 = this->v1 * this->v1; - float sqv2 = this->v2 * this->v2; - float sqv3 = this->v3 * this->v3; - float sqw = this->w * this->w; +Matrix<3, 1> Quaternion::ToEulerAngle() const { + float sqv1 = this->v1 * this->v1; + float sqv2 = this->v2 * this->v2; + float sqv3 = this->v3 * this->v3; + float sqw = this->w * this->w; - Matrix<3, 1> eulerAngle; - { - atan2(2.0 * (this->v1 * this->v2 + this->v3 * this->w), (sqv1 - sqv2 - sqv3 + sqw)); - asin(-2.0 * (this->v1 * this->v3 - this->v2 * this->w) / (sqv1 + sqv2 + sqv3 + sqw)); - atan2(2.0 * (this->v2 * this->v3 + this->v1 * this->w), (-sqv1 - sqv2 + sqv3 + sqw)); - }; - return eulerAngle; + Matrix<3, 1> eulerAngle; + { + atan2(2.0 * (this->v1 * this->v2 + this->v3 * this->w), + (sqv1 - sqv2 - sqv3 + sqw)); + asin(-2.0 * (this->v1 * this->v3 - this->v2 * this->w) / + (sqv1 + sqv2 + sqv3 + sqw)); + atan2(2.0 * (this->v2 * this->v3 + this->v1 * this->w), + (-sqv1 - sqv2 + sqv3 + sqw)); + }; + return eulerAngle; } \ No newline at end of file diff --git a/unit-tests/matrix-tests.cpp b/unit-tests/matrix-tests.cpp index a6e921c..fb0575e 100644 --- a/unit-tests/matrix-tests.cpp +++ b/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(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 +float matrixSum(const Matrix &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(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)); diff --git a/unit-tests/matrix-timing-tests.cpp b/unit-tests/matrix-timing-tests.cpp index 9c63afe..c48802f 100644 --- a/unit-tests/matrix-timing-tests.cpp +++ b/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(); } }