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

Cleanup

Browse Source
This commit is contained in:
Pascal Serrarens 2025-07-03 14:18:20 +02:00
parent 205dbeb877
commit 662a9340c2
3 changed files with 109 additions and 110 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

102
LinearAlgebra/Matrix.cpp
View File

@ -15,7 +15,7 @@ namespace LinearAlgebra {
Matrix1::Matrix1() {}
Matrix1::Matrix1(int size) : size(size) {
if (this->size == 0)
if (this->size <= 0)
data = nullptr;
else {
this->data = new float[size]();
@ -23,11 +23,26 @@ Matrix1::Matrix1(int size) : size(size) {
}
}
Matrix1::Matrix1(float* data, int size) : data(data), size(size) {
Matrix1::Matrix1(int size, float* data) : data(data), size(size) {
this->externalData = true;
}
int Matrix1::Size() const {
Matrix1::Matrix1(const Matrix1& m) : size(m.size) {
if (this->size <= 0)
this->data = nullptr;
else {
this->data = new float[this->size];
for (int ix = 0; ix < this->size; ++ix)
this->data[ix] = m.data[ix];
}
}
Matrix1::~Matrix1() {
delete[] data;
}
unsigned int Matrix1::Size() const {
return this->size;
}
@ -39,6 +54,28 @@ Matrix1 Matrix1::FromVector3(Vector3 v) {
return r;
}
Matrix1 Matrix1::Zero(unsigned int size) {
Matrix1 r(size);
r.Fill(0);
return r;
}
void Matrix1::Fill(float value) {
for (int ix = 0; ix < this->size; ix++)
this->data[ix] = value;
}
void Matrix1::Fill(float value, unsigned int start, unsigned int stop) {
if (start > stop || start > this->size)
return;
if (stop > this->size)
stop = this->size;
for (unsigned int ix = start; ix < stop; ix++)
this->data[ix] = value;
}
Vector3 Matrix1::ToVector3() {
return Vector3(this->data[0], this->data[1], this->data[2]);
}
@ -69,9 +106,18 @@ Matrix1 Matrix1::Slice(int start, int stop) {
return result;
}
void Matrix1::UpdateSlice(int start, int stop, const Matrix1& m) const {
for (int ix = start; ix < stop; ix++) {
this->data[ix] = m.data[ix - start];
void Matrix1::UpdateSlice(int start, int stop, const Matrix1& source) const {
if (start > stop || start > this->size || start > source.size)
return;
if (stop > this->size)
stop = this->size;
if (stop > source.size)
stop = source.size;
unsigned int sourceIx = 0;
for (int ix = start; ix < stop; ix++, sourceIx++) {
this->data[ix] = source.data[sourceIx];
}
}
@ -87,13 +133,6 @@ float& Matrix1::operator()(int ix) {
#pragma region Matrix2
// Function to check available heap memory
size_t getAvailableHeapMemory() {
// This is a simple example; you may need to implement a more robust method
// depending on your platform. For ESP32, you can use heap_4.c or similar.
return esp_get_free_heap_size(); // Example for ESP32
}
Matrix2::Matrix2() {}
Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) {
@ -105,32 +144,12 @@ Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) {
this->externalData = false;
return;
}
// Check available heap memory before allocation
size_t availableMemory = getAvailableHeapMemory();
if (availableMemory < this->nValues * sizeof(float)) {
std::cerr << "Error: Not enough memory to allocate Matrix2 of size "
<< this->nValues << " floats." << std::endl;
this->data = nullptr; // Handle memory allocation failure
this->externalData = false;
return;
}
UBaseType_t stack_size =
uxTaskGetStackHighWaterMark(NULL); // NULL to check the main task
if (stack_size < 1000)
std::cerr << "Stack High Water Mark: " << stack_size << std::endl;
try {
this->data = new float[this->nValues];
this->externalData = false;
} catch (const std::bad_alloc& e) {
std::cerr << "Memory allocation failed: " << e.what() << std::endl;
this->data = nullptr; // Handle memory allocation failure
this->externalData = false;
}
}
Matrix2::Matrix2(int nRows, int nCols, float* data)
Matrix2::Matrix2(int nRows, int nCols, float* data, float autoDestruct)
: nRows(nRows), nCols(nCols), data(data) {
this->nValues = nRows * nCols;
this->externalData = true;
@ -173,11 +192,12 @@ Matrix2::~Matrix2() {
}
}
Matrix2 Matrix2::Clone() const {
Matrix2 r = Matrix2(this->nRows, this->nCols);
for (int ix = 0; ix < this->nValues; ++ix)
r.data[ix] = this->data[ix];
return r;
unsigned int Matrix2::NRows() {
return this->nRows;
}
unsigned int Matrix2::NCols() {
return this->nCols;
}
// Move constructor
@ -218,9 +238,9 @@ float& Matrix2::operator()(int row, int col) {
return data[row * this->nCols + col];
}
void Matrix2::Clear() {
void Matrix2::Fill(float value) {
for (int ix = 0; ix < this->nValues; ix++)
this->data[ix] = 0;
this->data[ix] = value;
}
Matrix2 Matrix2::Identity(int size) {

105
LinearAlgebra/Matrix.h
View File

@ -11,9 +11,16 @@ class Matrix1 {
public:
Matrix1();
Matrix1(int size);
Matrix1(float* data, int size);
Matrix1(int size, float* data);
Matrix1(const Matrix1& m); // Make a clone
int Size() const;
~Matrix1();
unsigned int Size() const;
static Matrix1 Zero(unsigned int size);
void Fill(float value);
void Fill(float value, unsigned int start, unsigned int stop);
static Matrix1 FromVector3(Vector3 v);
Vector3 ToVector3();
@ -24,27 +31,10 @@ class Matrix1 {
const float& operator()(int ix) const;
float& operator()(int ix);
Matrix1 Slice(int start, int stop);
void UpdateSlice(int start, int stop, const Matrix1& m) const;
void Set(unsigned int start, unsigned int stop, float value) {
if (stop > this->size)
stop = this->size;
for (unsigned int ix = start; ix < stop; ix++)
this->data[ix] = value;
}
void Set(unsigned int start, unsigned int stop, Matrix1 source) {
if (stop > this->size)
stop = this->size;
if (stop > source.size)
stop = source.size;
// Do I need to check on start???
unsigned int sourceIx = 0;
for (unsigned int ix = start; ix < stop; ix++, sourceIx++)
this->data[ix] = source.data[sourceIx];
}
protected:
float* data = nullptr;
int size = 0;
@ -56,34 +46,47 @@ class Matrix1 {
/// @brief A 2-dimensional matrix of arbitrary size
class Matrix2 {
public:
int nRows = 0;
int nCols = 0;
int nValues = 0;
float* data = nullptr;
Matrix2();
Matrix2(int nRows, int nCols);
Matrix2(int nRows, int nCols, float* data);
Matrix2(int nRows, int nCols, float* data, float autoDestruct = false);
Matrix2(const Matrix2& m);
Matrix2& operator=(const Matrix2& other);
~Matrix2();
Matrix2 Clone() const;
unsigned int NRows();
unsigned int NCols();
static Matrix2 Zero(int nRows, int nCols);
void Clear();
void Fill(float value);
static Matrix2 Identity(int size);
static Matrix2 Diagonal(float f, int size);
static Matrix2 SkewMatrix(const Vector3& v);
Matrix2 Transpose() const;
Matrix2 Inverse();
const float& operator()(int row, int col) const;
float& operator()(int row, int col);
static Matrix2 Identity(int size);
Matrix2 GetRows(int from, int to);
Vector3 GetRow3(int rowIx);
static Matrix2 Diagonal(float f, int size);
void SetRow(int rowIx, Matrix1 source);
void SetRow3(int rowIx, Vector3 v);
static Matrix2 SkewMatrix(const Vector3& v);
Matrix2 Slice(int rawStart, int rowStop, int colStart, int colStop);
Matrix2 Transpose() const;
void UpdateSlice(int rowStart, int rowStop, const Matrix2& m) const;
void UpdateSlice(int rowStart,
int rowStop,
int colStart,
int colStop,
const Matrix2& m) const;
Matrix2 DeleteRows(int rowStart, int rowStop);
Matrix2 DeleteColumns(int colStart, int colStop);
Matrix2 operator-() const;
@ -110,15 +113,6 @@ class Matrix2 {
}
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/(const Matrix2& m, float f) {
Matrix2 r = Matrix2(m.nRows, m.nCols);
@ -133,34 +127,19 @@ class Matrix2 {
return r;
}
Matrix2 GetRows(int from, int to);
Vector3 GetRow3(int rowIx);
void SetRow(int rowIx, Matrix1 source);
void SetRow3(int rowIx, Vector3 v);
Matrix2 Slice(int rawStart, int rowStop, int colStart, int colStop);
void UpdateSlice(int rowStart, int rowStop, const Matrix2& m) const;
void UpdateSlice(int rowStart,
int rowStop,
int colStart,
int colStop,
const Matrix2& m) const;
Matrix2 Inverse();
Matrix2 DeleteRows(int rowStart, int rowStop);
Matrix2 DeleteColumns(int colStart, int colStop);
// We don't want these because they make impliciti copies which is inefficient
// move constructor and move assignment operator
// Matrix2(Matrix2&& other) noexcept;
// Matrix2& operator=(Matrix2&& other) noexcept;
static Matrix2 Omega(const Vector3& v);
public:
int nRows = 0;
int nCols = 0;
protected:
int nValues = 0;
float* data = nullptr;
private:
bool externalData = true;
};

6
LinearAlgebra/Quaternion.cpp
View File

@ -99,14 +99,12 @@ 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;
float* data = new float[9]; // 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);
@ -117,6 +115,8 @@ Matrix2 LinearAlgebra::Quaternion::ToRotationMatrix() {
data[2 * 3 + 1] = 2 * (y * z + w * x);
data[2 * 3 + 2] = 1 - 2 * (x * x + y * y);
Matrix2 r = Matrix2(3, 3, data, true);
return r;
}