From 39abc0247bf75bf2bc9b4bdf457af67da27226da Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 2 Apr 2025 10:03:31 +0200 Subject: [PATCH] Implemented integrate function --- LinearAlgebra/Matrix.cpp | 104 +++++++++++++++++++++++++++++++++------ LinearAlgebra/Matrix.h | 72 ++++++++++++++++++++++++--- 2 files changed, 154 insertions(+), 22 deletions(-) diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp index cbb7b7d..c1f60a5 100644 --- a/LinearAlgebra/Matrix.cpp +++ b/LinearAlgebra/Matrix.cpp @@ -1,8 +1,48 @@ #include "Matrix.h" #include +#pragma region Matrix1 + +LinearAlgebra::Matrix1::Matrix1(int size) : size(size) { + if (this->size == 0) + data = nullptr; + else { + this->data = new float[size](); + this->externalData = false; + } +} + +LinearAlgebra::Matrix1::Matrix1(float* data, int size) + : data(data), size(size) { + this->externalData = true; +} + +Matrix1 LinearAlgebra::Matrix1::FromQuaternion(Quaternion q) { + Matrix1 r = Matrix1(4); + float* data = r.data; + data[0] = q.x; + data[1] = q.y; + data[2] = q.z; + data[3] = q.w; + return r; +} + +Quaternion LinearAlgebra::Matrix1::ToQuaternion() { + return Quaternion( + this->data[0], + this->data[1], + this->data[2], + this->data[3] + ); +} + +// Matrix1 +#pragma endregion + #pragma region Matrix2 +Matrix2::Matrix2() {} + Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) { this->nValues = nRows * nCols; if (this->nValues == 0) @@ -14,7 +54,7 @@ Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) { } Matrix2::Matrix2(float* data, int nRows, int nCols) - : nRows(nRows), nCols(nCols), data(data){ + : nRows(nRows), nCols(nCols), data(data) { this->nValues = nRows * nCols; this->externalData = true; } @@ -25,22 +65,26 @@ Matrix2::~Matrix2() { } // Move constructor -Matrix2::Matrix2(Matrix2&& other) noexcept - : nRows(other.nRows), nCols(other.nCols), nValues(other.nValues), data(other.data) { - other.data = nullptr; // Set the other object's pointer to nullptr to avoid double deletion +Matrix2::Matrix2(Matrix2&& other) noexcept + : nRows(other.nRows), + nCols(other.nCols), + nValues(other.nValues), + data(other.data) { + other.data = nullptr; // Set the other object's pointer to nullptr to avoid + // double deletion } // Move assignment operator Matrix2& Matrix2::operator=(Matrix2&& other) noexcept { - if (this != &other) { - delete[] data; // Clean up current data - nRows = other.nRows; - nCols = other.nCols; - nValues = other.nValues; - data = other.data; - other.data = nullptr; // Avoid double deletion - } - return *this; + if (this != &other) { + delete[] data; // Clean up current data + nRows = other.nRows; + nCols = other.nCols; + nValues = other.nValues; + data = other.data; + other.data = nullptr; // Avoid double deletion + } + return *this; } Matrix2 Matrix2::Zero(int nRows, int nCols) { @@ -84,13 +128,20 @@ Matrix2 LinearAlgebra::Matrix2::operator-() const { return r; } +Matrix2 LinearAlgebra::Matrix2::operator+(const Matrix2& v) const { + Matrix2 r = Matrix2(this->nRows, this->nCols); + for (int ix = 0; ix < r.nValues; ix++) + r.data[ix] = this->data[ix] + v.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; + // int BRows = B.nRows; for (int i = 0; i < ARows; ++i) { // Pre-compute row offsets @@ -101,7 +152,8 @@ Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const { std::cout << " 0"; int BIndex = j; for (int k = 0; k < ACols; ++k) { - std::cout << " + " << this->data[ARowOffset + k] << " * " << B.data[BIndex]; + std::cout << " + " << this->data[ARowOffset + k] << " * " + << B.data[BIndex]; sum += this->data[ARowOffset + k] * B.data[BIndex]; BIndex += BCols; } @@ -121,10 +173,30 @@ void LinearAlgebra::Matrix2::SetSlice(int rowStart, 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]; } } +/// @brief Compute the Omega matrix of a 3D vector +/// @param v The vector +/// @return 4x4 Omega matrix +Matrix2 LinearAlgebra::Matrix2::Omega(const Vector3& v) { + Matrix2 r = Matrix2::Zero(4, 4); + r.SetSlice(0, 3, 0, 3, -Matrix2::SkewMatrix(v)); + + // set last row to -v + int ix = 3 * 4; + r.data[ix++] = -v.x; + r.data[ix++] = -v.y; + r.data[ix] = -v.z; + + // Set last column to v + ix = 3; + r.data[ix += 4] = v.x; + r.data[ix += 4] = v.y; + r.data[ix] = v.z; + return r; +} + // Matrix2 #pragma endregion diff --git a/LinearAlgebra/Matrix.h b/LinearAlgebra/Matrix.h index cd5bd76..f66eeeb 100644 --- a/LinearAlgebra/Matrix.h +++ b/LinearAlgebra/Matrix.h @@ -1,22 +1,40 @@ #ifndef MATRIX_H #define MATRIX_H +#include "Quaternion.h" #include "Vector3.h" namespace LinearAlgebra { +/// @brief A 1-dimensional matrix or vector of arbitrary size +class Matrix1 { + public: + int size = 0; + float* data = nullptr; + + Matrix1(int size); + Matrix1(float* data, int size); + + static Matrix1 FromQuaternion(Quaternion q); + Quaternion ToQuaternion(); + + private: + bool externalData = true; +}; + +/// @brief A 2-dimensional matrix of arbitrary size class Matrix2 { public: int nRows = 0; int nCols = 0; int nValues = 0; float* data = nullptr; - bool externalData = true; + Matrix2(); Matrix2(int nRows, int nCols); Matrix2(float* data, int nRows, int nCols); - ~Matrix2(); + ~Matrix2(); static Matrix2 Zero(int nRows, int nCols); @@ -28,13 +46,55 @@ class Matrix2 { Matrix2 operator-() const; - Matrix2 operator*(const Matrix2& m) const; + /// @brief Add a matrix to this matrix + /// @param m The matrix to add to this matrix + /// @return The result of the addition + Matrix2 operator+(const Matrix2& v) const; - void SetSlice(int rowStart, int rowStop, int colStart, int colStop, const Matrix2& m) const; -//private: - // move constructor and move assignment operator + Matrix2 operator*(const Matrix2& m) const; + friend Matrix2 operator*(const Matrix2& m, float f) { + Matrix2 r = Matrix2(m.nRows, m.nCols); + for (int ix = 0; ix < r.nValues; ix++) + r.data[ix] = m.data[ix] * f; + return r; + } + friend Matrix2 operator*(float f, const Matrix2& m) { + Matrix2 r = Matrix2(m.nRows, m.nCols); + for (int ix = 0; ix < r.nValues; ix++) + r.data[ix] = f * m.data[ix]; + return r; + } + + friend Matrix1 operator*(const Matrix2& m, const Matrix1& v) { + Matrix1 r = Matrix1(m.nRows); + for (int rowIx = 0; rowIx < m.nRows; rowIx++) { + int mRowIx = rowIx * m.nCols; + for (int colIx = 0; colIx < m.nCols; colIx++) + r.data[rowIx] += m.data[mRowIx + colIx] * v.data[rowIx]; + } + return r; + } + friend Matrix2 operator*(float f, const Matrix2& v) { + Matrix2 r = Matrix2(v.nRows, v.nCols); + for (int ix = 0; ix < r.nValues; ix++) + r.data[ix] = f * v.data[ix]; + return r; + } + + void SetSlice(int rowStart, + int rowStop, + int colStart, + int colStop, + const Matrix2& m) const; + // private: + // move constructor and move assignment operator Matrix2(Matrix2&& other) noexcept; Matrix2& operator=(Matrix2&& other) noexcept; + + static Matrix2 Omega(const Vector3& v); + + private: + bool externalData = true; }; /// @brief Single precision float matrix