perf(math): build matrix results without zero-filling them first (#349)

* perf(math): build matrix results without zero-filling them first

Every Matrix operator that returns a new matrix value-initialised its
result (data = {}) and then overwrote every element. Without the O3
pragmas #348 removed, a consumer that builds at -O2 keeps those loops
rolled, so the zeroing survives. For a 3x3 result it is a memset call,
and the Arm GNU toolchain's newlib-nano memset stores one byte at a
time.

e-foc runs an RLS update in the control interrupt during mechanical
identification. Its covariance update, (P - g g^T / d) / lambda,
builds four 3x3 temporaries. In e-foc's SIL scenario that times the
control interrupt through a full calibration, the slowest execution
rose from 237 to 377 cycles of the emulated 25 MHz clock
(arm-none-eabi-gcc 15.2.1). The four memset calls were 596 of its 1892
instructions.

- Matrix gains a private constructor that leaves the elements
  uninitialised. operator+, operator-, the matrix and scalar products,
  Transpose, GetBlock and GetColumn build their results with it, since
  each writes every element.
- The default and initializer-list constructors still zero-fill, so a
  default-constructed or partly listed matrix keeps its zeros.
- A test evaluates those operators in constant expressions, so a
  compiler that checks (Clang does, GCC does not) rejects an operator
  that leaves an element unwritten.

The RLS update keeps its matrix expression and now makes no memset
calls. The same scenario peaks at 250 cycles.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01HKhRVwu8yvxdKJgNo17oNc

* fix(math): build the matrix product's result in a member function

The matrix product is a friend function of its left operand's Matrix
specialisation. It built its result, a Matrix of another shape, with
that shape's private constructor. GCC and Clang allowed it, but MSVC
rejects it (C2248): friendship goes to the Matrix specialisations, not
to their friend functions.

The operator now calls a private member function, Multiply, which
builds the result in place, as Transpose, GetBlock and GetColumn
already do. Members of every Matrix specialisation are friends of the
others, so every compiler accepts it.

A static helper that returned the uninitialised result was not used:
GCC folds such an argument-less constexpr call into a zero-filled
constant, which brings back the memset calls.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01HKhRVwu8yvxdKJgNo17oNc

---------

Co-authored-by: Claude <noreply@anthropic.com>

Author: gabrielfrasantos

numerical/math/Matrix.hpp

@@ -63,15 +63,15 @@ namespace math
 
         [[nodiscard]] OPTIMIZE_FOR_SPEED friend constexpr Matrix operator+(const Matrix& lhs, const Matrix& rhs)
         {
-            Matrix result;
+            Matrix result{ Uninitialized{} };
             for (size_type i = 0; i < size; ++i)
                 result.data[i] = lhs.data[i] + rhs.data[i];
             return result;
         }
 
         [[nodiscard]] OPTIMIZE_FOR_SPEED friend constexpr Matrix operator-(const Matrix& lhs, const Matrix& rhs)
         {
-            Matrix result;
+            Matrix result{ Uninitialized{} };
             for (size_type i = 0; i < size; ++i)
                 result.data[i] = lhs.data[i] - rhs.data[i];
             return result;
@@ -80,23 +80,12 @@ namespace math
         template<size_t RhsCols>
         [[nodiscard]] OPTIMIZE_FOR_SPEED friend constexpr Matrix<T, Rows, RhsCols> operator*(const Matrix& lhs, const Matrix<T, Cols, RhsCols>& rhs)
         {
-            Matrix<T, Rows, RhsCols> result;
-            for (size_type i = 0; i < Rows; ++i)
-            {
-                for (size_type j = 0; j < RhsCols; ++j)
-                {
-                    T sum{};
-                    for (size_type k = 0; k < Cols; ++k)
-                        sum += lhs.at(i, k) * rhs.at(k, j);
-                    result.at(i, j) = sum;
-                }
-            }
-            return result;
+            return lhs.Multiply(rhs);
         }
 
         [[nodiscard]] OPTIMIZE_FOR_SPEED friend constexpr Matrix operator*(const Matrix& lhs, const T& scalar)
         {
-            Matrix result;
+            Matrix result{ Uninitialized{} };
             for (size_type i = 0; i < size; ++i)
                 result.data[i] = lhs.data[i] * scalar;
             return result;
@@ -121,7 +110,19 @@ namespace math
         [[nodiscard]] constexpr Matrix<T, Rows, 1> GetColumn(size_type col) const;
 
     private:
-        std::array<T, Rows * Cols> data = {};
+        template<typename, size_t, size_t>
+        friend class Matrix;
+
+        struct Uninitialized
+        {};
+
+        constexpr explicit Matrix(Uninitialized) noexcept
+        {}
+
+        template<size_t RhsCols>
+        [[nodiscard]] constexpr Matrix<T, Rows, RhsCols> Multiply(const Matrix<T, Cols, RhsCols>& rhs) const;
+
+        std::array<T, Rows * Cols> data;
     };
 
     template<typename T, typename... U>
@@ -141,10 +142,13 @@ namespace math
     }
 
     template<typename T, size_t Rows, size_t Cols>
-    constexpr Matrix<T, Rows, Cols>::Matrix() noexcept = default;
+    constexpr Matrix<T, Rows, Cols>::Matrix() noexcept
+        : data{}
+    {}
 
     template<typename T, size_t Rows, size_t Cols>
     OPTIMIZE_FOR_SPEED constexpr Matrix<T, Rows, Cols>::Matrix(std::initializer_list<std::initializer_list<T>> init)
+        : data{}
     {
         size_t row = 0;
         for (const auto& row_list : init)
@@ -263,11 +267,30 @@ namespace math
         return *this;
     }
 
+    template<typename T, size_t Rows, size_t Cols>
+    template<size_t RhsCols>
+    OPTIMIZE_FOR_SPEED constexpr Matrix<T, Rows, RhsCols>
+    Matrix<T, Rows, Cols>::Multiply(const Matrix<T, Cols, RhsCols>& rhs) const
+    {
+        Matrix<T, Rows, RhsCols> result{ typename Matrix<T, Rows, RhsCols>::Uninitialized{} };
+        for (size_type i = 0; i < Rows; ++i)
+        {
+            for (size_type j = 0; j < RhsCols; ++j)
+            {
+                T sum{};
+                for (size_type k = 0; k < Cols; ++k)
+                    sum += at(i, k) * rhs.at(k, j);
+                result.at(i, j) = sum;
+            }
+        }
+        return result;
+    }
+
     template<typename T, size_t Rows, size_t Cols>
     OPTIMIZE_FOR_SPEED constexpr Matrix<T, Cols, Rows>
     Matrix<T, Rows, Cols>::Transpose() const
     {
-        Matrix<T, Cols, Rows> result;
+        Matrix<T, Cols, Rows> result{ typename Matrix<T, Cols, Rows>::Uninitialized{} };
         for (size_type i = 0; i < Rows; ++i)
             for (size_type j = 0; j < Cols; ++j)
                 result.at(j, i) = at(i, j);
@@ -328,7 +351,7 @@ namespace math
     {
         static_assert(BlockRows <= Rows && BlockCols <= Cols,
             "Requested block exceeds source matrix dimensions");
-        Matrix<T, BlockRows, BlockCols> result;
+        Matrix<T, BlockRows, BlockCols> result{ typename Matrix<T, BlockRows, BlockCols>::Uninitialized{} };
         for (size_type r = 0; r < BlockRows; ++r)
             for (size_type c = 0; c < BlockCols; ++c)
                 result.at(r, c) = at(rowOffset + r, colOffset + c);
@@ -339,7 +362,7 @@ namespace math
     [[nodiscard]] OPTIMIZE_FOR_SPEED constexpr Matrix<T, Rows, 1>
     Matrix<T, Rows, Cols>::GetColumn(size_type col) const
     {
-        Vector<T, Rows> result;
+        Vector<T, Rows> result{ typename Vector<T, Rows>::Uninitialized{} };
         for (size_type r = 0; r < Rows; ++r)
             result.at(r, 0) = at(r, col);
         return result;

numerical/math/test/TestMatrix.cpp

@@ -427,6 +427,24 @@ TEST_F(MatrixFloatShapeTest, three_by_three_compound_assignment_and_trace)
     EXPECT_NEAR(square3.Transpose().at(0, 2), 7.0f, math::Tolerance<float>());
 }
 
+TEST_F(MatrixFloatShapeTest, operators_write_every_element_of_their_result)
+{
+    constexpr math::Matrix<float, 3, 1> column{ { 1.0f }, { 2.0f }, { 3.0f } };
+    constexpr auto outer = column * column.Transpose();
+    constexpr auto sum = outer + outer;
+    constexpr auto difference = sum - outer;
+    constexpr auto scaled = difference * 0.5f;
+    constexpr auto block = scaled.GetBlock<2, 2>(1, 1);
+    constexpr auto lastColumn = scaled.GetColumn(2);
+
+    EXPECT_NEAR(outer.at(2, 1), 6.0f, math::Tolerance<float>());
+    EXPECT_NEAR(sum.at(0, 2), 6.0f, math::Tolerance<float>());
+    EXPECT_NEAR(difference.at(1, 1), 4.0f, math::Tolerance<float>());
+    EXPECT_NEAR(scaled.at(2, 2), 4.5f, math::Tolerance<float>());
+    EXPECT_NEAR(block.at(1, 0), 3.0f, math::Tolerance<float>());
+    EXPECT_NEAR(lastColumn.at(0, 0), 1.5f, math::Tolerance<float>());
+}
+
 TEST_F(MatrixFloatShapeTest, non_square_shapes_transpose_and_index)
 {
     math::Matrix<float, 1, 4> row{ { 1.0f, 2.0f, 3.0f, 4.0f } };