Matrix.hpp

Go to the documentation of this file.

/******************************************************************************
* Copyright (c) ATTX LLC 2024. All Rights Reserved.
*
* This software and associated documentation (the "Software") are the 
* proprietary and confidential information of ATTX, LLC. The Software is 
* furnished under a license agreement between ATTX and the user organization 
* and may be used or copied only in accordance with the terms of the agreement.
* Refer to 'license/attx_license.adoc' for standard license terms.
*
* EXPORT CONTROL NOTICE: THIS SOFTWARE MAY INCLUDE CONTENT CONTROLLED UNDER THE
* INTERNATIONAL TRAFFIC IN ARMS REGULATIONS (ITAR) OR THE EXPORT ADMINISTRATION 
* REGULATIONS (EAR99). No part of the Software may be used, reproduced, or 
* transmitted in any form or by any means, for any purpose, without the express 
* written permission of ATTX, LLC. 
******************************************************************************/
/*
Matrix math header file
 
Author: Alex Reynolds
*/
#ifndef CORE_MATRIX_HPP
#define CORE_MATRIX_HPP
 
#include <cmath>
#include <array>
#include <cstring>
 
#include "clockwerkerrors.h"
#include "safemath.h"
#include "utils/stringutils.hpp"
 
namespace clockwerk {
    const uint32 MAX_CHARS_PER_MATRIX_STRING = 1000;

    template <uint32 R, uint32 C> 
    class Matrix {
    public:
        Matrix();

        Matrix(floating_point elements);

        Matrix(const floating_point(&initial)[R][C]);

        Matrix(const Matrix<R, C> &initial);

        Matrix(const std::array<std::array<floating_point, C>, R> &initial);

        ~Matrix() {}

#if CLOCKWERK_ALLOW_STD_STRING
        std::string dump() const {
            char retval[MAX_CHARS_PER_MATRIX_STRING]; 
            str(retval, MAX_CHARS_PER_MATRIX_STRING); 
            return std::string(retval);
        }
#endif

        int16 str(char* output, size_t size) const;

        int16 fromStr(const char* val);

        int16 set(uint32 row, uint32 col, const floating_point &value);

        int16 get(uint32 row, uint32 col, floating_point &result) const;

        void setFromArray(const floating_point* start_ptr, bool column_major = false);

        void getAsArray(floating_point* start_ptr, bool column_major = false) const;

        floating_point get(uint32 row, uint32 col) const;

        void getCopy(Matrix<R, C> &result) const;

        Matrix<R, C>& operator=(const Matrix<R, C>& other);

        floating_point* operator [](uint32 idx);
        

        std::pair<uint32, uint32> size() const {return std::pair<uint32, uint32>(R, C);}

        void max(floating_point &result, std::pair<uint32, uint32> &index) const;

        void min(floating_point &result, std::pair<uint32, uint32> &index) const;


        int16 det(floating_point &result) const;

        int16 chol(Matrix<R, C>& retval) const;
        Matrix<R, C> chol() const {Matrix<R, C> tmp; chol(tmp); return tmp;}

        int16 inverse(Matrix<R, C> &result) const;
        Matrix<R, C> inverse() const {Matrix<R, C> tmp; inverse(tmp); return tmp;}
 
        int16 pseudoinverse(Matrix<C, R> &result) const;
        Matrix<C, R> pseudoinverse() const {Matrix<C, R> tmp; pseudoinverse(tmp); return tmp; }

        void transpose(Matrix<C, R> &result) const;
        Matrix<C, R> transpose() const;

        int16 trace(floating_point &result) const;

        void setToZeros();

        int16 identity();
        int16 eye() {return identity();}

        std::array<std::array<floating_point, C>, R> values;
 
    protected:
        int16 _checkLookupBoundaries(uint32 start_r, uint32 end_r, 
                                uint32 start_c, uint32 end_c) const;

        int16 _LUPDecompose(floating_point *A[R], uint32 P[R+1]) const;
 
    };
 
    template <uint32 R, uint32 C> 
    Matrix<R, C>::Matrix() {
        uint32 i, j;
        for(i = 0; i < R; i++) {
            for(j = 0; j < C; j++) {
                values[i][j] = 0.0;
            }
        }
    }
 
    template <uint32 R, uint32 C> 
    Matrix<R, C>::Matrix(floating_point elements) {
        uint32 i, j;
        for(i = 0; i < R; i++) {
            for(j = 0; j < C; j++) {
                values[i][j] = elements;
            }
        }
    }
 
    template <uint32 R, uint32 C> 
    Matrix<R, C>::Matrix(const floating_point(&initial)[R][C]) {
        
        uint32 i, j;
        for(i = 0; i < R; i++) {
            for(j = 0; j < C; j++) {
                values[i][j] = initial[i][j];
            }
        }
    }
 
    template <uint32 R, uint32 C> 
    Matrix<R, C>::Matrix(const Matrix<R, C> &initial) {
        uint32 i, j;
        for(i = 0; i < R; i++) {
            for(j = 0; j < C; j++) {
                values[i][j] = initial.values[i][j];
            }
        } 
    }
 
    template <uint32 R, uint32 C> 
    Matrix<R, C>::Matrix(const std::array<std::array<floating_point, C>, R> &initial) {
        uint32 i, j;
        for(i = 0; i < R; i++) {
            for(j = 0; j < C; j++) {
                values[i][j] = initial[i][j]; 
            }
        } 
    }
 
    template <uint32 R, uint32 C> 
    floating_point* Matrix<R, C>::operator [](uint32 idx) {
        // Return pointer to our array element
        return &values[idx][0];
    }
 
    template <uint32 R, uint32 C> 
    int16 Matrix<R, C>::str(char* output, size_t size) const {
        if (output == nullptr) return ERROR_NULLPTR;
 
        if(size < 3) {
            return ERROR_DIMENSIONS;
        }
        strcpy(output, "[[");
 
        // Configure initial string
        bool dimensions_error = false;
        uint32 val_idx = 0;
        int num_chars_filled = 2;
        while (val_idx < R && !dimensions_error) {
            // Call number to string conversion, starting at open space in our buffer
            int16 err = numberToString(values[val_idx], output + num_chars_filled, size - num_chars_filled);
            if(err) {dimensions_error = true;}
 
            if(!dimensions_error) {
                // Calculate updated parameters
                num_chars_filled = strnlen(output, size);
 
                // Add in comma to separate values
                if(size - num_chars_filled > 2) {
                    if(val_idx < R - 1) {
                        strcpy(output + num_chars_filled, "][");
                    } else {
                        strcpy(output + num_chars_filled, "]]");
                    }
                    num_chars_filled += 2;
                } else {
                    dimensions_error = true;
                }
            }
 
            val_idx++;
        }
 
        if(dimensions_error) {return ERROR_DIMENSIONS;}
        return NO_ERROR;
    }
 
    template <uint32 R, uint32 C> 
    int16 Matrix<R, C>::fromStr(const char* val) {
        if (val == nullptr || val[0] != '[') {
            return ERROR_DIMENSIONS;
        }
 
        const char* ptr = val + 1;  // skip the initial '['
        int16 err = NO_ERROR;
 
        for (uint32 i = 0; i < R; ++i) {
            // Find the start of the row
            const char* row_start = strchr(ptr, '[');
            if (!row_start) {
                return ERROR_DIMENSIONS;
            }
            row_start++;
 
            // Find the end of the row
            const char* row_end = strchr(row_start, ']');
            if (!row_end) return ERROR_DIMENSIONS;
 
            // Copy row content into a buffer for parsing
            size_t row_len = row_end - row_start;
 
            char row_buf[row_len];
            std::strncpy(row_buf, row_start, row_len);
            row_buf[row_len] = '\0';
 
            // Parse the row string into the matrix row
            err = setArrayFromString(row_buf, values[i].data(), C);
            if (err) {
                return err;
            }
 
            ptr = row_end + 1;  // move past ']'
        }
 
        return NO_ERROR;
    }
 
    template <uint32 R, uint32 C> 
    int16 Matrix<R, C>::set(uint32 row, uint32 col, const floating_point &value){
        if(_checkLookupBoundaries(row, row, col, col)) {
            return ERROR_DIMENSIONS;
        } 

        values[row][col] = value;
        return NO_ERROR;
    }
 
    template <uint32 R, uint32 C> 
    int16 Matrix<R, C>::get(uint32 row, uint32 col, floating_point &result) const{
        if(_checkLookupBoundaries(row, row, col, col)) {
            return ERROR_DIMENSIONS;
        } 

        result = values[row][col];
        return NO_ERROR;
    }
 
    template <uint32 R, uint32 C> 
    floating_point Matrix<R, C>::get(uint32 row, uint32 col) const{
        return values[row][col];
    }
 
    template <uint32 R, uint32 C> 
    void Matrix<R, C>::setFromArray(const floating_point* start_ptr, bool column_major) {
        const floating_point* ptr = start_ptr;
        uint32 i, j;
        if(column_major) {
            for(i = 0; i < R; i++) {
                for(j = 0; j < C; j++) {
                    values[j][i] = *ptr;
                    ptr++;
                }
            } 
        } else {
            for(i = 0; i < R; i++) {
                for(j = 0; j < C; j++) {
                    values[i][j] = *ptr;
                    ptr++;
                }
            } 
        }
    }
 
    template <uint32 R, uint32 C> 
    void Matrix<R, C>::getAsArray(floating_point* start_ptr, bool column_major) const {
        floating_point* ptr = start_ptr;
        uint32 i, j;
        if(column_major) {
            for(i = 0; i < R; i++) {
                for(j = 0; j < C; j++) {
                    *ptr = values[j][i];
                    ptr++;
                }
            } 
        } else {
            for(i = 0; i < R; i++) {
                for(j = 0; j < C; j++) {
                    *ptr = values[i][j];
                    ptr++;
                }
            } 
        }
    }
 
    template <uint32 R, uint32 C> 
    void Matrix<R, C>::getCopy(Matrix<R, C> &result) const {
 
        uint32 i, j;
        for(i = 0; i < R; i++) {
            for(j = 0; j < C; j++) {
                result.values[i][j] = values[i][j];
            }
        } 
    }
 
    template <uint32 R, uint32 C> 
    Matrix<R, C>& Matrix<R, C>::operator=(const Matrix<R, C>& other)
    {
        uint32 i, j;
        for(i = 0; i < R; i++) {
            for(j = 0; j < C; j++) {
                this->values[i][j] = other.values[i][j];
            }
        } 
        return *this;
    }
 
    template <uint32 R, uint32 C> 
    void Matrix<R, C>::max(floating_point &result, std::pair<uint32, uint32> &index) const {
        uint32 i, j;
        index.first = 0;
        index.second = 0;
        result = values[0][0];      
        for(i = 0; i < R; i++) {
            for(j = 0; j < C; j++) {
                if(values[i][j] > result) {
                    result = values[i][j];
                    index.first = i;
                    index.second = j;
                }
            }
        } 
    }
 
    template <uint32 R, uint32 C> 
    void Matrix<R, C>::min(floating_point &result, std::pair<uint32, uint32> &index) const {
        uint32 i, j;
        index.first = 0;
        index.second = 0;
        result = values[0][0];      
        for(i = 0; i < R; i++) {
            for(j = 0; j < C; j++) {
                if(values[i][j] < result) {
                    result = values[i][j];
                    index.first = i;
                    index.second = j;
                }
            }
        } 
    }
 
    template <uint32 R, uint32 C> 
    int16 Matrix<R, C>::det(floating_point &result) const {

        uint32 i, j, err;

        floating_point A[R][C];
        floating_point *decomposed[R];
        for(i = 0; i < R; i++) {
            decomposed[i] = A[i];
            for(j = 0; j < C; j++) {
                A[i][j] = values[i][j];
            }
        }

        uint32 P[R+1];

        err = _LUPDecompose(decomposed, P);
        if(err == ERROR_POORLY_CONDITIONED) {
            result = 0;
            return NO_ERROR;
        } else if(err) {
            return err;
        }

        result = decomposed[0][0];
        for(i = 1; i < R; i++) {
            result *= decomposed[i][i];
        }
        if((P[R] - R)%2) {
            result = -result;
        } 
 
        return NO_ERROR;
    }
 
    template <uint32 R, uint32 C>
    int16 Matrix<R, C>::chol(Matrix<R, C>& retval) const{
        // Only square matrices are choleskible
        if (R != C) {
            return ERROR_DIMENSIONS;
        }
 
        // Zero output.
        for (uint32 i = 0; i < R; ++i) {
            for (uint32 j = 0; j < C; ++j) {
                retval.values[i][j] = 0.0;
            }
        }
 
        // Compute MATLAB-style upper-triangular R such that R' * R = A.
        for (uint32 j = 0; j < C; ++j)
        {
            floating_point sum = 0.0;
            for (uint32 k = 0; k < j; ++k) {
                sum += retval.values[k][j]*retval.values[k][j];
            }
 
            const floating_point diag = values[j][j] - sum;
 
            // SPD check / conditioning
            if (diag <= TOLERANCE_CONDITIONING) {
                return ERROR_POORLY_CONDITIONED;
            }
            // Note no need for safe sqrt here becuase we just verified that diag isn't < 0
            retval.values[j][j] = static_cast<floating_point>(std::sqrt(diag));
 
            // Off-diagonal in row j (upper triangle): R(j,i) for i = j+1..C-1
            floating_point rjj = retval.values[j][j];
            for (uint32 i = j + 1; i < C; ++i) {
                sum = 0.0;
                for (uint32 k = 0; k < j; ++k) {
                    sum += retval.values[k][j]*retval.values[k][i];
                }
 
                // Note here we enforce symmetry in our matrix by taking the mean of the off diag terms
                floating_point aji = (values[j][i] + values[i][j])*0.5;
                retval.values[j][i] = (aji - sum)/rjj;  // No need for safe divide because we verified rjj.
            }
        }
 
        return NO_ERROR;
    }
 
    template <uint32 R, uint32 C> 
    int16 Matrix<R, C>::inverse(Matrix<R, C> &result) const {
        // Only square matrices are invertible
        if (R != C) {
            result = 0.0;      // deterministic output on error
            return ERROR_DIMENSIONS;
        }
 
        // Trivial 1x1 case: inverse of a scalar
        if (R == 1) {
            result = 0.0;
            int16 err = safeDivide(static_cast<floating_point>(1.0),
                                values[0][0],
                                result.values[0][0]);
            if (err) {
                result = 0.0;
                return err;    // e.g. divide-by-zero / poorly conditioned
            }
            return NO_ERROR;
        }
 
        // Ensure deterministic content even if we early-exit
        result.setToZeros();
 
        // Local copy of the matrix as a 2D array for in-place LU factorization
        floating_point A[R][C];
        floating_point* decomposed[R];
 
        for (uint32 i = 0; i < R; ++i) {
            decomposed[i] = A[i];
            for (uint32 j = 0; j < C; ++j) {
                A[i][j] = values[i][j];
            }
        }
 
        // Permutation "matrix" P
        // P[0..R-1] = permuted row indices, P[R] = pivot count
        uint32 P[R + 1];
 
        // LU decomposition with partial pivoting and conditioning check
        int16 err = _LUPDecompose(decomposed, P);
        if (err) {
            // ERROR_POORLY_CONDITIONED, ERROR_DIMENSIONS, etc.
            result = 0.0;
            return err;
        }
 
        // Solve A * X = I using the LU factors in 'decomposed' and permutation P.
        // We fill result column-by-column (each column is solution of A * x_j = e_j).
        for (uint32 j = 0; j < R; ++j) {
            // Forward substitution: L * y = P * e_j
            // Start with y = P*e_j encoded into result.values[*][j]
            for (uint32 i = 0; i < R; ++i) {
                // P[i] tells which row of identity goes here
                result.values[i][j] = (P[i] == j) ? static_cast<floating_point>(1.0)
                                                : static_cast<floating_point>(0.0);
                for (uint32 k = 0; k < i; ++k) {
                    result.values[i][j] -= decomposed[i][k] * result.values[k][j];
                }
            }
 
            // Back substitution: U * x = y
            for (uint32 idx = 1; idx <= R; ++idx) {
                uint32 i = R - idx;  // count backwards: R-1, R-2, ..., 0
 
                for (uint32 k = i + 1; k < R; ++k) {
                    result.values[i][j] -= decomposed[i][k] * result.values[k][j];
                }
 
                // Divide by U(i,i)
                floating_point tmp;
                err = safeDivide(result.values[i][j], decomposed[i][i], tmp);
                if (err) {
                    result = 0.0;
                    return err;
                }
                result.values[i][j] = tmp;
            }
        }
 
        return NO_ERROR;
    }
 
    template <uint32 R, uint32 C>
    int16 Matrix<R, C>::pseudoinverse(Matrix<C, R> &result) const {
 
        //--- CASE 1: Tall matrix (R >= C) -----------------------------------------
        if (R > C)
        {
            // Compute ATA = Aᵀ A  (C×C)
            Matrix<C, C> ATA;
            for(uint32 i = 0; i < C; i++)
                for(uint32 j = 0; j < C; j++) {
                    for(uint32 k = 0; k < R; k++) {
                        ATA.values[i][j] += values[k][i] * values[k][j];
                }
            }
 
            // Invert ATA
            Matrix<C, C> ATA_inv;
            int16 err = ATA.inverse(ATA_inv);
            if(err) {return err;}        // Singular / bad conditioning
 
            // Compute A⁺ = (AᵀA)⁻¹ Aᵀ
            for(uint32 i = 0; i < C; i++)
                for(uint32 j = 0; j < R; j++) {
                    result.values[i][j] = 0.0;
                    for(uint32 k = 0; k < C; k++) {
                        result.values[i][j] += ATA_inv.values[i][k] * values[j][k];
                    }
                }
 
            return NO_ERROR;
        }
 
        //--- CASE 2: Wide matrix (C > R) ------------------------------------------
        else if (R < C)
        {
            // Compute AAT = A Aᵀ  (R×R)
            Matrix<R, R> AAT;
            for(uint32 i = 0; i < R; i++)
                for(uint32 j = 0; j < R; j++) {
                    for(uint32 k = 0; k < C; k++) {
                        AAT.values[i][j] += values[i][k] * values[j][k];
                    }
            }
 
            // Invert AAT
            Matrix<R, R> AAT_inv;
            int16 err = AAT.inverse(AAT_inv);
            if(err) {return err;}
 
            // Compute A⁺ = Aᵀ (A Aᵀ)⁻¹
            for(uint32 i = 0; i < C; i++)
                for(uint32 j = 0; j < R; j++) {
                    result.values[i][j] = 0.0;
                    for(uint32 k = 0; k < R; k++) {
                        result.values[i][j] += values[k][i] * AAT_inv.values[k][j];
                    }
                }
 
            return NO_ERROR;
        }
        //--- CASE 3: Square matrix (C = R) ------------------------------------------
        else {
            Matrix<R, C> inv;
            int16 err = inverse(inv);
            if(err) {return err;}
            
            // Copy values element by element (safe when R==C)
            for(uint32 i = 0; i < R; i++) {
                for(uint32 j = 0; j < C; j++) {
                    result.values[i][j] = inv.values[i][j];
                }
            }
            
            return NO_ERROR;
        }
    }
 
    template <uint32 R, uint32 C> 
    void Matrix<R, C>::transpose(Matrix<C, R> &result) const {
 
        uint32 i, j;
        for(i = 0; i < R; i++) {
            for(j = 0; j < C; j++) {
                result.values[j][i] = values[i][j];
            }
        } 
    }
    template <uint32 R, uint32 C> 
    Matrix<C, R> Matrix<R, C>::transpose() const {
        Matrix<C, R> tmp;
        transpose(tmp);
        return tmp;
    }
 
    template <uint32 R, uint32 C> 
    int16 Matrix<R, C>::trace(floating_point &result) const {
        if(R != C) {
            return ERROR_DIMENSIONS;
        }
 
        result = 0.0;
        uint32 i;
        for(i = 0; i < R; i++) {
            result += values[i][i];
        }
        return NO_ERROR;
    }
 
    template <uint32 R, uint32 C> 
    void Matrix<R, C>::setToZeros() {
        uint32 i, j;
        for(i = 0; i < R; i++) {
            for(j = 0; j < C; j++) {
                values[j][i] = 0;
            }
        } 
    }
 
    template <uint32 R, uint32 C> 
    int16 Matrix<R, C>::identity() {
        if(C != R) {return ERROR_DIMENSIONS;}
        uint32 i, j;
        for(i = 0; i < R; i++) {
            for(j = 0; j < C; j++) {
                if(i == j) {
                    values[i][i] = 1.0;
                } else {
                    values[i][j] = 0.0;
                }
            }
        } 
 
        return NO_ERROR;
    }
 
    template <uint32 R, uint32 C>  
    int16 Matrix<R, C>::_checkLookupBoundaries(uint32 start_r, uint32 end_r, 
                                                uint32 start_c, uint32 end_c) const{
        if(start_r > end_r || start_c > end_c) {
            return ERROR_DIMENSIONS;
        }
        if(end_r >= R || end_c >= C) { 
            return ERROR_DIMENSIONS;
        }
        return NO_ERROR;
    }
 
    template<uint32 R, uint32 C>
    int16 Matrix<R, C>::_LUPDecompose(floating_point *A[R], uint32 P[R+1]) const {
        uint32 i, j, k, imax; 
        floating_point max_A, *ptr, abs_A;

        for (k = 0; k <= R; k++) {
            P[k] = k;
        }

        for (i = 0; i < R; i++) {
            max_A = 0.0;
            imax = i;

            for (k = i; k < R; k++) {
                abs_A = std::abs(A[k][i]);      
                if (abs_A > max_A) { 
                    max_A = abs_A;
                    imax = k;
                }
            }

            if (max_A < TOLERANCE_CONDITIONING) 
                return ERROR_POORLY_CONDITIONED; //failure, matrix is degenerate

            if (imax != i) {
                P[R]++;

                j = P[i];
                P[i] = P[imax];
                P[imax] = j;

                ptr = A[i];
                A[i] = A[imax];
                A[imax] = ptr;
            }

            for (j = i+1; j < R; j++) {
                safeDivide(A[j][i], A[i][i], A[j][i]);
                for (k = i+1; k < R; k++) {
                    A[j][k] -= A[j][i] * A[i][k];
                }
            }
        }

        return NO_ERROR;
    }
 
}
 
#endif