diff --git a/CMakeLists.txt b/CMakeLists.txt index 934c5ad..f660483 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,8 +1,8 @@ cmake_minimum_required(VERSION 3.13) # CMake version check if(ESP_PLATFORM) idf_component_register( - SRC_DIRS "." - INCLUDE_DIRS "." + SRC_DIRS "." "LinearAlgebra" + INCLUDE_DIRS "." "LinearAlgebra" ) else() project(RoboidControl) diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp index 2b83a1d..689f86d 100644 --- a/LinearAlgebra/Matrix.cpp +++ b/LinearAlgebra/Matrix.cpp @@ -1,6 +1,105 @@ #include "Matrix.h" -template <> MatrixOf::MatrixOf(unsigned int rows, unsigned int cols) { +#pragma region Matrix2 + +Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) { + this->nValues = nRows * nCols; + data = new float[nValues](); +} + +Matrix2::Matrix2(float* data, int nRows, int nCols) + : nRows(nRows), nCols(nCols), data(data) { + this->nValues = nRows * nCols; +} + +Matrix2::~Matrix2() { + delete[] data; +} + +Matrix2 Matrix2::Zero(int nRows, int nCols) { + Matrix2 m = Matrix2(nRows, nCols); + for (int ix = 0; ix < m.nValues; ix++) + m.data[ix] = 0; + return m; +} + +Matrix2 Matrix2::Identity(int size) { + return Diagonal(1, size); +} + +Matrix2 Matrix2::Diagonal(float f, int size) { + Matrix2 r = Matrix2(size, size); + float* data = r.data; + int valueIx = 0; + for (int ix = 0; ix < r.nValues; ix++) { + data[valueIx] = f; + valueIx += size + 1; + } + return r; +} + +Matrix2 Matrix2::SkewMatrix(const Vector3& v) { + Matrix2 r = Matrix2(3, 3); + float* data = r.data; + data[0 * 3 + 1] = -v.z; // result(0, 1) + data[0 * 3 + 2] = v.y; // result(0, 2) + data[1 * 3 + 0] = v.z; // result(1, 0) + data[1 * 3 + 2] = -v.x; // result(1, 2) + data[2 * 3 + 0] = -v.y; // result(2, 0) + data[2 * 3 + 1] = v.x; // result(2, 1) + return r; +} + +Matrix2 LinearAlgebra::Matrix2::operator-() const { + Matrix2 r = Matrix2(this->nRows, this->nCols); + for (int ix = 0; ix < r.nValues; ix++) + r.data[ix] = -this->data[ix]; + return r; +} + +Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const { + Matrix2 r = Matrix2(this->nRows, B.nCols); + + int ACols = this->nCols; + int BCols = B.nCols; + int ARows = this->nRows; + //int BRows = B.nRows; + + for (int i = 0; i < ARows; ++i) { + // Pre-compute row offsets + int ARowOffset = i * ACols; // ARowOffset is constant for each row of A + int BColOffset = i * BCols; // BColOffset is constant for each row of B + for (int j = 0; j < BCols; ++j) { + float sum = 0; + int BIndex = j; + for (int k = 0; k < ACols; ++k) { + sum += this->data[ARowOffset + k] * B.data[BIndex]; + BIndex += BCols; + } + r.data[BColOffset + j] = sum; + } + } + return r; +} + +void LinearAlgebra::Matrix2::SetSlice(int rowStart, + int rowStop, + int colStart, + int colStop, + const Matrix2& m) const { + for (int i = rowStart; i < rowStop; i++) { + for (int j = colStart; j < colStop; j++) + this->data[i * this->nCols + j] = + m.data[(i - rowStart) * m.nCols + (j - colStart)]; + // this->data[i, j] = m.data[i - rowStart, j - colStart]; + } +} + +// Matrix2 +#pragma endregion + +template <> +MatrixOf::MatrixOf(unsigned int rows, unsigned int cols) { if (rows <= 0 || cols <= 0) { this->rows = 0; this->cols = 0; @@ -14,15 +113,17 @@ template <> MatrixOf::MatrixOf(unsigned int rows, unsigned int cols) { this->data = new float[matrixSize]{0.0f}; } -template <> MatrixOf::MatrixOf(Vector3 v) : MatrixOf(3, 1) { +template <> +MatrixOf::MatrixOf(Vector3 v) : MatrixOf(3, 1) { Set(0, 0, v.Right()); Set(1, 0, v.Up()); Set(2, 0, v.Forward()); } template <> -void MatrixOf::Multiply(const MatrixOf *m1, - const MatrixOf *m2, MatrixOf *r) { +void MatrixOf::Multiply(const MatrixOf* m1, + const MatrixOf* m2, + MatrixOf* r) { for (unsigned int rowIx1 = 0; rowIx1 < m1->rows; rowIx1++) { for (unsigned int colIx2 = 0; colIx2 < m2->cols; colIx2++) { unsigned int rDataIx = colIx2 * m2->cols + rowIx1; @@ -37,7 +138,7 @@ void MatrixOf::Multiply(const MatrixOf *m1, } template <> -Vector3 MatrixOf::Multiply(const MatrixOf *m, Vector3 v) { +Vector3 MatrixOf::Multiply(const MatrixOf* m, Vector3 v) { MatrixOf v_m = MatrixOf(v); MatrixOf r_m = MatrixOf(3, 1); @@ -47,10 +148,11 @@ Vector3 MatrixOf::Multiply(const MatrixOf *m, Vector3 v) { return r; } -template Vector3 MatrixOf::operator*(const Vector3 v) const { - float *vData = new float[3]{v.Right(), v.Up(), v.Forward()}; +template +Vector3 MatrixOf::operator*(const Vector3 v) const { + float* vData = new float[3]{v.Right(), v.Up(), v.Forward()}; MatrixOf v_m = MatrixOf(3, 1, vData); - float *rData = new float[3]{}; + float* rData = new float[3]{}; MatrixOf r_m = MatrixOf(3, 1, rData); Multiply(this, &v_m, &r_m); diff --git a/LinearAlgebra/Matrix.h b/LinearAlgebra/Matrix.h index 925cb5a..5348f58 100644 --- a/LinearAlgebra/Matrix.h +++ b/LinearAlgebra/Matrix.h @@ -5,6 +5,33 @@ namespace LinearAlgebra { +class Matrix2 { + public: + int nRows = 0; + int nCols = 0; + int nValues = 0; + float* data = nullptr; + + Matrix2(int nRows, int nCols); + Matrix2(float* data, int nRows, int nCols); + + ~Matrix2(); + + static Matrix2 Zero(int nRows, int nCols); + + static Matrix2 Identity(int size); + + static Matrix2 Diagonal(float f, int size); + + static Matrix2 SkewMatrix(const Vector3& v); + + Matrix2 operator-() const; + + Matrix2 operator*(const Matrix2& m) const; + + void SetSlice(int rowStart, int rowStop, int colStart, int colStop, const Matrix2& m) const; +}; + /// @brief Single precision float matrix template class MatrixOf { diff --git a/LinearAlgebra/Quaternion.cpp b/LinearAlgebra/Quaternion.cpp index 1a7352e..dda8c18 100644 --- a/LinearAlgebra/Quaternion.cpp +++ b/LinearAlgebra/Quaternion.cpp @@ -6,6 +6,7 @@ #include #include #include "Angle.h" +#include "Matrix.h" #include "Vector3.h" void CopyQuat(const Quat& q1, Quat& q2) { @@ -97,6 +98,28 @@ Vector3 Quaternion::ToAngles(const Quaternion& q1) { } } +Matrix2 LinearAlgebra::Quaternion::ToRotationMatrix() { + Matrix2 r = Matrix2(3, 3); + + float x = this->x; + float y = this->y; + float z = this->z; + float w = this->w; + + float* data = r.data; + data[0 * 3 + 0] = 1 - 2 * (y * y + z * z); + data[0 * 3 + 1] = 2 * (x * y - w * z); + data[0 * 3 + 2] = 2 * (x * z + w * y); + data[1 * 3 + 0] = 2 * (x * y + w * z); + data[1 * 3 + 1] = 1 - 2 * (x * x + z * z); + data[1 * 3 + 2] = 2 * (y * z - w * x); + data[2 * 3 + 0] = 2 * (x * z - w * y); + data[2 * 3 + 1] = 2 * (y * z + w * x); + data[2 * 3 + 2] = 1 - 2 * (x * x + y * y); + + return r; +} + Quaternion Quaternion::operator*(const Quaternion& r2) const { return Quaternion( this->x * r2.w + this->y * r2.z - this->z * r2.y + this->w * r2.x, diff --git a/LinearAlgebra/Quaternion.h b/LinearAlgebra/Quaternion.h index d1ddd4e..1687dc9 100644 --- a/LinearAlgebra/Quaternion.h +++ b/LinearAlgebra/Quaternion.h @@ -34,6 +34,8 @@ typedef struct Quat { namespace LinearAlgebra { +class Matrix2; + /// /// A quaternion /// @@ -89,6 +91,8 @@ struct Quaternion : Quat { /// The euler angles performed in the order: Z, X, Y static Vector3 ToAngles(const Quaternion& q); + Matrix2 ToRotationMatrix(); + /// /// Rotate a vector using this quaterion /// diff --git a/LinearAlgebra/Vector3.h b/LinearAlgebra/Vector3.h index 8775b2d..914d72a 100644 --- a/LinearAlgebra/Vector3.h +++ b/LinearAlgebra/Vector3.h @@ -14,7 +14,7 @@ extern "C" { /// This is a C-style implementation /// This uses the right-handed coordinate system. typedef struct Vec3 { - protected: + public: /// /// The right axis of the vector /// diff --git a/LinearAlgebra/test/Matrix_test.cc b/LinearAlgebra/test/Matrix_test.cc index a1267cf..a782afc 100644 --- a/LinearAlgebra/test/Matrix_test.cc +++ b/LinearAlgebra/test/Matrix_test.cc @@ -5,6 +5,58 @@ #include "Matrix.h" +TEST(Matrix2, Multiplication) { + // Test 1: Multiplying two 2x2 matrices + float dataA[] = {1, 2, 3, 4}; + float dataB[] = {5, 6, 7, 8}; + Matrix2 A(dataA, 2, 2); + Matrix2 B(dataB, 2, 2); + + Matrix2 result = A * B; + + float expectedData[] = {19, 22, 43, 50}; + for (int i = 0; i < 4; ++i) { + //assert(result.data[i] == expectedData[i]); + EXPECT_TRUE(result.data[i] == expectedData[i]); + } + std::cout << "Test 1 passed: 2x2 matrix multiplication.\n"; + + // Test 2: Multiplying a 3x2 matrix with a 2x3 matrix + float dataC[] = {1, 2, 3, 4, 5, 6}; + float dataD[] = {7, 8, 9, 10, 11, 12}; + Matrix2 C(dataC, 3, 2); + Matrix2 D(dataD, 2, 3); + + Matrix2 result2 = C * D; + + float expectedData2[] = {29, 32, 35, 65, 72, 79, 101, 112, 123}; + for (int i = 0; i < 9; ++i) { + assert(result2.data[i] == expectedData2[i]); + EXPECT_TRUE(result2.data[i] == expectedData2[i]); + } + std::cout << "Test 2 passed: 3x2 * 2x3 matrix multiplication.\n"; + + // Test 3: Multiplying with a zero matrix + Matrix2 zeroMatrix = Matrix2::Zero(2, 2); + Matrix2 result3 = A * zeroMatrix; + + for (int i = 0; i < 4; ++i) { + assert(result3.data[i] == 0); + EXPECT_TRUE(result3.data[i] == 0); + } + std::cout << "Test 3 passed: Multiplication with zero matrix.\n"; + + // Test 4: Multiplying with an identity matrix + Matrix2 identityMatrix = Matrix2::Identity(2); + Matrix2 result4 = A * identityMatrix; + + for (int i = 0; i < 4; ++i) { + assert(result4.data[i] == A.data[i]); + EXPECT_TRUE(result4.data[i] == A.data[i]); + } + std::cout << "Test 4 passed: Multiplication with identity matrix.\n"; +} + TEST(MatrixSingle, Init) { // zero MatrixOf m0 = MatrixOf(0, 0);