Cleanup

LinearAlgebra/Matrix.cpp

@@ -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 {
+void Matrix1::UpdateSlice(int start, int stop, const Matrix1& source) const {
-  for (int ix = start; ix < stop; ix++) {
+  if (start > stop || start > this->size || start > source.size)
-    this->data[ix] = m.data[ix - start];
+    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 =
+  this->data = new float[this->nValues];
-      uxTaskGetStackHighWaterMark(NULL);  // NULL to check the main task
+  this->externalData = false;
-  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 {
+unsigned int Matrix2::NRows() {
-  Matrix2 r = Matrix2(this->nRows, this->nCols);
+  return this->nRows;
-  for (int ix = 0; ix < this->nValues; ++ix)
+}
-    r.data[ix] = this->data[ix];
+
-  return r;
+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) {

LinearAlgebra/Matrix.h

@@ -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);
+    //  move constructor and move assignment operator
-  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;
 };

LinearAlgebra/Quaternion.cpp

@@ -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;
 }