matrixmath.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
-----------------------
This file defines basic functions for mathematical operations on
the matrix class
Rather than raising an error on fail cases (i.e. divide by zero),
the following libraries are designed to check for errors and return
corresponding codes.
 
Note: The naming convention observed for matrix math is as follows:
- Capital letters represent matrices
- Lowercase letters represent scalars
- All vectors are 1-dimensional matrices for the purpose of math
- Pass by reference return values are all named "result"
- The letter used in math indicates the order of operations - A/a
  comes first, then B/b. A/a or B/b always represent the "other"
  value in an operation
 
Author: Alex Reynolds
*/
 
#ifndef CORE_MATRIXMATH_HPP
#define CORE_MATRIXMATH_HPP
 
#include "Matrix.hpp"
#include "clockwerkerrors.h"
 
namespace clockwerk {


    template<uint32 R1, uint32 C1R2, uint32 C2>
    void multiply(const Matrix<R1, C1R2> &A, const Matrix<C1R2, C2> &B, Matrix<R1, C2> &result) {
        if(static_cast<const void*>(&A) == static_cast<const void*>(&result)) {
            Matrix<R1, C1R2> a_cpy = A;
            uint32 i, j, k;
            for(i = 0; i < R1; i++) {
                for(j = 0; j < C2; j++) {
                    result.values[i][j] = 0.0;
                    for(k = 0; k < C1R2; k++) {
                        result.values[i][j] += a_cpy.values[i][k]*B.values[k][j];
                    }
                }
            }
 
            return;
        }
        if(static_cast<const void*>(&B) == static_cast<const void*>(&result)) {
            Matrix<C1R2, C2> b_cpy = B;
            uint32 i, j, k;
            for(i = 0; i < R1; i++) {
                for(j = 0; j < C2; j++) {
                    result.values[i][j] = 0.0;
                    for(k = 0; k < C1R2; k++) {
                        result.values[i][j] += A.values[i][k]*b_cpy.values[k][j];
                    }
                }
            }
 
            return;
        }

        uint32 i, j, k;
        for(i = 0; i < R1; i++) {
            for(j = 0; j < C2; j++) {
                result.values[i][j] = 0.0;
                for(k = 0; k < C1R2; k++) {
                    result.values[i][j] += A.values[i][k]*B.values[k][j];
                }
            }
        }
    }

    template<uint32 R, uint32 C>
    void multiply(const floating_point &a, const Matrix<R, C> &A, Matrix<R, C> &result) {

        uint32 i, j;
        for(i = 0; i < R; i++) {
            for(j = 0; j < C; j++) {
                result.values[i][j] = a*A.values[i][j];
            }
        }
    }

    template<uint32 R, uint32 C>
    void eMultiply(const Matrix<R, C> &A, const Matrix<R, C> &B, Matrix<R, C> &result) {

        uint32 i, j;
        for(i = 0; i < R; i++) {
            for(j = 0; j < C; j++) {
                result.values[i][j] = A.values[i][j]*B.values[i][j];
            }
        }
    }

    template<uint32 R, uint32 C>
    void add(const floating_point &a, const Matrix<R, C> &A, Matrix<R, C> &result) {

        uint32 i, j;
        for(i = 0; i < R; i++) {
            for(j = 0; j < C; j++) {
                result.values[i][j] = a + A.values[i][j];
            }
        }
    }

    template<uint32 R, uint32 C>
    void eAdd(const Matrix<R, C> &A, const Matrix<R, C> &B, Matrix<R, C> &result) {

        uint32 i, j;
        for(i = 0; i < R; i++) {
            for(j = 0; j < C; j++) {
                result.values[i][j] = A.values[i][j] + B.values[i][j];
            }
        }
    }

    template<uint32 R, uint32 C>
    void eSubtract(Matrix<R, C> A, Matrix<R, C> B, Matrix<R, C> &result) {

        uint32 i, j;
        for(i = 0; i < R; i++) {
            for(j = 0; j < C; j++) {
                result.values[i][j] = A.values[i][j] - B.values[i][j];
            }
        }
    }

    template<uint32 R1, uint32 C1R2, uint32 C2>
    Matrix<R1, C2> operator*(const Matrix<R1, C1R2> &A, const Matrix<C1R2, C2> &B) {
        Matrix<R1, C2> result;
        multiply(A, B, result);
        return result;
    }

    template<uint32 R, uint32 C>
    Matrix<R, C> operator*(const floating_point &a, const Matrix<R, C> &A) {
        Matrix<R, C> result;
        multiply(a, A, result);
        return result;
    }
    template<uint32 R, uint32 C>
    Matrix<R, C> operator*(const Matrix<R, C> &A, const floating_point &a) {
        Matrix<R, C> result;
        multiply(a, A, result);
        return result;
    }

    template<uint32 R, uint32 C>
    Matrix<R, C> operator+(const Matrix<R, C> &A, const Matrix<R, C> &B) {
        Matrix<R, C> result;
        eAdd(A, B, result);
        return result;
    }

    template<uint32 R, uint32 C>
    Matrix<R, C> operator+(const floating_point &a, const Matrix<R, C> &A) {
        Matrix<R, C> result;
        add(a, A, result);
        return result;
    }
    template<uint32 R, uint32 C>
    Matrix<R, C> operator+(const Matrix<R, C> &A, const floating_point &a) {
        Matrix<R, C> result;
        add(a, A, result);
        return result;
    }

    template<uint32 R, uint32 C>
    Matrix<R, C> operator-(const Matrix<R, C> &A, const Matrix<R, C> &B) {
        Matrix<R, C> result;
        eSubtract(A, B, result);
        return result;
    }

    template <uint32 M, uint32 K>
    int16 cholSolve(const Matrix<M, M> &A, const Matrix<M, K> &B, Matrix<M, K> &X) {
        // Cholesky decompose: this = Rᵀ R  (upper triangular R, MATLAB convention)
        Matrix<M, M> R;
        int16 err = A.chol(R);
        if (err) { return err; }
 
        // Solve column by column: for each column b of B, solve Rᵀ R x = b
        floating_point Y[M];  // scratch for forward solve
 
        for (uint32 col = 0; col < K; col++) {
            // Forward solve: Rᵀ y = b  (Rᵀ is lower triangular)
            for (uint32 i = 0; i < M; i++) {
                floating_point sum = B.get(i, col);
                for (uint32 k = 0; k < i; k++) {
                    sum -= R.get(k, i) * Y[k];  // Rᵀ[i][k] = R[k][i]
                }
                err = safeDivide(sum, R.get(i, i), Y[i]); 
                if(err) {return err;}
            }
 
            // Back solve: R x = y  (R is upper triangular)
            for (int32 i = M - 1; i >= 0; i--) {
                floating_point sum = Y[i];
                for (uint32 k = i + 1; k < M; k++) {
                    sum -= R.get(i, k) * X.get(k, col);
                }
                floating_point tmp;
                err = safeDivide(sum, R.get(i, i), tmp);
                if(err) {return err;}
                X.set(i, col, tmp);
            }
        }
 
        return NO_ERROR;
    }
 
}
 
#endif