matrix_math.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
// MATRIX MATH LIBRARY FOR TEENSY 4.1
// 
// Code By: Michael Wrona
// Created: 14 Feb 2021
// ----------------------------------------------------------------------------
#pragma once
 
#include <Arduino.h>
#include <math.h>
#include "hummingbird_config.h"
#include "maths/math_functs.h"
#include "maths/matrices.h"
#include "maths/vectors.h"
 
 
#if defined(DEBUG) && defined(DEBUG_PORT)
    /* Enable/disable printing debug messages to DEBUG_PORT */
    #define MATRIX_MATH_DEBUG
#endif
 
 
/* VECTOR FUNCTIONS */
void VectorfFill(float *vec, float fill, size_t n);
void VectorfAdd(float *c, float *a, float *b, size_t n);
void VectorfAccumulate(float *a, float *b, size_t n);
void VectorfSubtract(float *c, float *a, float *b, size_t n);
 
/* MATRIX FUNCTIONS */
void MatrixFill(float fill, float *A, size_t rows, size_t cols);
void MatrixTranspose(float *A, float *At, size_t arows, size_t acols);
void MatrixTransposeSquare(float *A, size_t n);
 
// Addition
void MatrixAdd(float *C, float *A, float *B, size_t rows, size_t cols);
void MatrixAddIdentity(float *A, size_t rows, size_t cols);
void MatrixAccumulate(float *A, float *B, size_t rows, size_t cols);
 
// Subtraction
void MatrixSubtract(float *C, float *A, float *B, size_t rows, size_t cols);
void MatrixSubtractIdentity(float *A, size_t rows, size_t cols);
void MatrixSubAccumulate(float *A, float *B, size_t rows, size_t cols);
void MatrixNegate(float *A, size_t rows, size_t cols);
 
// Multiplication
void MatrixVectorfMult(float *outVec, float *A, float *b, size_t rows, size_t cols);
void MatrixMultiply(float *C, float *A, float *B, size_t aRows, size_t aCols, size_t bRows, size_t bCols);
void MatrixMultiply_ABt(float *C, float *A, float *B, size_t arows, size_t acols, size_t brows);
 
// Invert
bool MatrixInverseCholesky(float *A, size_t n);
bool _MatrixCholeskyDecomp(float *A, size_t n);
bool _MatrixLowerTriangularInverse(float *A, size_t n);
// bool MatrixIsPosDef(float *A, size_t rows, size_t cols);
 
// #ifdef MATRIX_MATH_DEBUG
    // void PrintVectorf(float *a, size_t n);
    // void PrintMatrix(float *A, size_t r, size_t c);
// #endif