matd.h

Go to the documentation of this file.

/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
All rights reserved.
This software was developed in the APRIL Robotics Lab under the
direction of Edwin Olson, ebolson@umich.edu. This software may be
available under alternative licensing terms; contact the address above.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this
   list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
The views and conclusions contained in the software and documentation are those
of the authors and should not be interpreted as representing official policies,
either expressed or implied, of the Regents of The University of Michigan.
*/

#pragma once

#include <assert.h>
#include <stddef.h>
#include <string.h>

#ifdef __cplusplus
extern "C" {
#endif

typedef struct
{
    unsigned int nrows, ncols;
    double data[];
//    double *data;
} matd_t;

#define MATD_ALLOC(name, nrows, ncols) double name ## _storage [nrows*ncols]; matd_t name = { .nrows = nrows, .ncols = ncols, .data = &name ## _storage };

#define MATD_EPS 1e-8

#define MATD_EL(m, row, col) (m)->data[((row)*(m)->ncols + (col))]

matd_t *matd_create(int rows, int cols);

matd_t *matd_create_data(int rows, int cols, const double *data);

matd_t *matd_create_dataf(int rows, int cols, const float *data);

matd_t *matd_identity(int dim);

matd_t *matd_create_scalar(double v);

double matd_get(const matd_t *m, int row, int col);

void matd_put(matd_t *m, int row, int col, double value);

double matd_get_scalar(const matd_t *m);

void matd_put_scalar(matd_t *m, double value);

matd_t *matd_copy(const matd_t *m);

matd_t *matd_select(const matd_t *a, int r0, int r1, int c0, int c1);

void matd_print(const matd_t *m, const char *fmt);

void matd_print_transpose(const matd_t *m, const char *fmt);

matd_t *matd_add(const matd_t *a, const matd_t *b);

void matd_add_inplace(matd_t *a, const matd_t *b);

matd_t *matd_subtract(const matd_t *a, const matd_t *b);

void matd_subtract_inplace(matd_t *a, const matd_t *b);

matd_t *matd_scale(const matd_t *a, double s);

void matd_scale_inplace(matd_t *a, double s);

matd_t *matd_multiply(const matd_t *a, const matd_t *b);

matd_t *matd_transpose(const matd_t *a);

double matd_det(const matd_t *a);

matd_t *matd_inverse(const matd_t *a);

static inline void matd_set_data(matd_t *m, const double *data)
{
    memcpy(m->data, data, m->nrows * m->ncols * sizeof(double));
}

static inline int matd_is_scalar(const matd_t *a)
{
    assert(a != NULL);
    return a->ncols <= 1 && a->nrows <= 1;
}

static inline int matd_is_vector(const matd_t *a)
{
    assert(a != NULL);
    return a->ncols == 1 || a->nrows == 1;
}

static inline int matd_is_vector_len(const matd_t *a, int len)
{
    assert(a != NULL);
    return (a->ncols == 1 && a->nrows == (unsigned int)len) || (a->ncols == (unsigned int)len && a->nrows == 1);
}

double matd_vec_mag(const matd_t *a);

double matd_vec_dist(const matd_t *a, const matd_t *b);


double matd_vec_dist_n(const matd_t *a, const matd_t *b, int n);

double matd_vec_dot_product(const matd_t *a, const matd_t *b);

matd_t *matd_vec_normalize(const matd_t *a);

matd_t *matd_crossproduct(const matd_t *a, const matd_t *b);

double matd_err_inf(const matd_t *a, const matd_t *b);

matd_t *matd_op(const char *expr, ...);

void matd_destroy(matd_t *m);

typedef struct
{
    matd_t *U;
    matd_t *S;
    matd_t *V;
} matd_svd_t;

matd_svd_t matd_svd(matd_t *A);

#define MATD_SVD_NO_WARNINGS 1
    matd_svd_t matd_svd_flags(matd_t *A, int flags);

// PLU Decomposition

// All square matrices (even singular ones) have a partially-pivoted
// LU decomposition such that A = PLU, where P is a permutation
// matrix, L is a lower triangular matrix, and U is an upper
// triangular matrix.
//
typedef struct
{
    // was the input matrix singular? When a zero pivot is found, this
    // flag is set to indicate that this has happened.
    int singular;

    unsigned int *piv; // permutation indices
    int pivsign; // either +1 or -1

    // The matd_plu_t object returned "owns" the enclosed LU matrix. It
    // is not expected that the returned object is itself useful to
    // users: it contains the L and U information all smushed
    // together.
    matd_t *lu; // combined L and U matrices, permuted so they can be triangular.
} matd_plu_t;

matd_plu_t *matd_plu(const matd_t *a);
void matd_plu_destroy(matd_plu_t *mlu);
double matd_plu_det(const matd_plu_t *lu);
matd_t *matd_plu_p(const matd_plu_t *lu);
matd_t *matd_plu_l(const matd_plu_t *lu);
matd_t *matd_plu_u(const matd_plu_t *lu);
matd_t *matd_plu_solve(const matd_plu_t *mlu, const matd_t *b);

// uses LU decomposition internally.
matd_t *matd_solve(matd_t *A, matd_t *b);

// Cholesky Factorization

//matd_t *matd_cholesky(const matd_t *A);

typedef struct
{
    int is_spd;
    matd_t *u;
} matd_chol_t;

matd_chol_t *matd_chol(matd_t *A);
matd_t *matd_chol_solve(const matd_chol_t *chol, const matd_t *b);
void matd_chol_destroy(matd_chol_t *chol);
// only sensible on PSD matrices
matd_t *matd_chol_inverse(matd_t *a);

void matd_ltransposetriangle_solve(matd_t *u, const double *b, double *x);
void matd_ltriangle_solve(matd_t *u, const double *b, double *x);
void matd_utriangle_solve(matd_t *u, const double *b, double *x);


double matd_max(matd_t *m);

#ifdef __cplusplus
}
#endif