RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

First steps implementation Matrix operations

Browse Source
This commit is contained in:
Pascal Serrarens 2025-04-01 15:24:20 +02:00
parent fdf4d3aff6
commit 91ff401ecf
7 changed files with 219 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

@ -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)

118
LinearAlgebra/Matrix.cpp
View File

@ -1,6 +1,105 @@
#include "Matrix.h"
template <> MatrixOf<float>::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<float>::MatrixOf(unsigned int rows, unsigned int cols) {
if (rows <= 0 || cols <= 0) {
this->rows = 0;
this->cols = 0;
@ -14,15 +113,17 @@ template <> MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) {
this->data = new float[matrixSize]{0.0f};
}
template <> MatrixOf<float>::MatrixOf(Vector3 v) : MatrixOf(3, 1) {
template <>
MatrixOf<float>::MatrixOf(Vector3 v) : MatrixOf(3, 1) {
Set(0, 0, v.Right());
Set(1, 0, v.Up());
Set(2, 0, v.Forward());
}
template <>
void MatrixOf<float>::Multiply(const MatrixOf<float> *m1,
const MatrixOf<float> *m2, MatrixOf<float> *r) {
void MatrixOf<float>::Multiply(const MatrixOf<float>* m1,
const MatrixOf<float>* m2,
MatrixOf<float>* 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<float>::Multiply(const MatrixOf<float> *m1,
}
template <>
Vector3 MatrixOf<float>::Multiply(const MatrixOf<float> *m, Vector3 v) {
Vector3 MatrixOf<float>::Multiply(const MatrixOf<float>* m, Vector3 v) {
MatrixOf<float> v_m = MatrixOf<float>(v);
MatrixOf<float> r_m = MatrixOf<float>(3, 1);
@ -47,10 +148,11 @@ Vector3 MatrixOf<float>::Multiply(const MatrixOf<float> *m, Vector3 v) {
return r;
}
template <typename T> Vector3 MatrixOf<T>::operator*(const Vector3 v) const {
float *vData = new float[3]{v.Right(), v.Up(), v.Forward()};
template <typename T>
Vector3 MatrixOf<T>::operator*(const Vector3 v) const {
float* vData = new float[3]{v.Right(), v.Up(), v.Forward()};
MatrixOf<float> v_m = MatrixOf<float>(3, 1, vData);
float *rData = new float[3]{};
float* rData = new float[3]{};
MatrixOf<float> r_m = MatrixOf<float>(3, 1, rData);
Multiply(this, &v_m, &r_m);

27
LinearAlgebra/Matrix.h
View File

@ -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 <typename T>
class MatrixOf {

23
LinearAlgebra/Quaternion.cpp
View File

@ -6,6 +6,7 @@
#include <float.h>
#include <math.h>
#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,

4
LinearAlgebra/Quaternion.h
View File

@ -34,6 +34,8 @@ typedef struct Quat {
namespace LinearAlgebra {
class Matrix2;
/// <summary>
/// A quaternion
/// </summary>
@ -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();
/// <summary>
/// Rotate a vector using this quaterion
/// </summary>

2
LinearAlgebra/Vector3.h
View File

@ -14,7 +14,7 @@ extern "C" {
/// This is a C-style implementation
/// This uses the right-handed coordinate system.
typedef struct Vec3 {
protected:
public:
/// <summary>
/// The right axis of the vector
/// </summary>

52
LinearAlgebra/test/Matrix_test.cc
View File

@ -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<float> m0 = MatrixOf<float>(0, 0);