/* * This file is part of the ESO Common Pipeline Library * Copyright (C) 2001-2008,2014 European Southern Observatory * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program; if not, write to the Free Software * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA */ /** * @defgroup cpl_matrix Matrices * * This module provides functions to create, destroy and use a @em cpl_matrix. * The elements of a @em cpl_matrix with M rows and N columns are counted * from 0,0 to M-1,N-1. The matrix element 0,0 is the one at the upper left * corner of a matrix. The CPL matrix functions work properly only in the * case the matrices elements do not contain garbage (such as @c NaN or * infinity). * * @par Synopsis: * @code * #include * @endcode */ #include "cpl_matrix.h" #include #include #include #include /**@{*/ #ifndef inline #define inline /* inline */ #endif #define dtiny(x, t) ((x) < 0.0 ? (x) >= -(t) : (x) <= (t)) #define CPL_ATTR_NONNULL typedef enum { CPL_ERROR_NONE = 0, CPL_ERROR_NULL_INPUT, CPL_ERROR_ILLEGAL_INPUT, CPL_ERROR_INCOMPATIBLE_INPUT, CPL_ERROR_ILLEGAL_OUTPUT, CPL_ERROR_ACCESS_OUT_OF_RANGE, CPL_ERROR_SINGULAR_MATRIX, CPL_ERROR_DIVISION_BY_ZERO, } cpl_error_code; /* * The cpl_matrix type: */ struct _cpl_matrix { cpl_size nc; cpl_size nr; double *m; }; struct _cpl_vector { cpl_size n; double * d; } ; #define CX_MIN(a, b) (((a) < (b)) ? (a) : (b)) #define cpl_ensure_code(a, ret) \ do { \ if (!(a)) \ return ret; \ } while(0) #define cpl_ensure(a, code, ret) \ do { \ if (!(a)) \ return ret; \ } while(0) static void * cpl_malloc(size_t sz) { return malloc(sz); } static void * cpl_calloc(size_t sz, size_t elsz) { return calloc(sz, elsz); } static void cpl_free(void * p) { free(p); } static cpl_error_code cpl_error_set_(cpl_error_code code) { return code; } static cpl_error_code cpl_error_set(const char *a , cpl_error_code code) { return code; } static void cpl_error_set_where(const char *a) { } static cpl_error_code cpl_error_set_where_(void) { return 0; } static cpl_error_code cpl_error_get_code(void) { return 0; } /* minimal vector, no error checking */ cpl_vector * cpl_vector_new(cpl_size n) { cpl_vector * v = cpl_malloc(sizeof(*v)); v->d = cpl_malloc(n * sizeof(v->d[0])); v->n = n; return v; } cpl_vector * cpl_vector_duplicate(const cpl_vector * a) { cpl_vector *v = cpl_vector_new(a->n); memcpy(v->d, a->d, a->n * sizeof(a->d[0])); return v; } cpl_vector * cpl_vector_wrap(long n, double *d) { cpl_vector *v = cpl_malloc(sizeof(*v)); v->n = n; v->d = d; return v; } void cpl_vector_delete(cpl_vector * v) { if (v == NULL) return; cpl_free(v->d); cpl_free(v); } void cpl_vector_unwrap(cpl_vector * v) { if (v == NULL) return; cpl_free(v); } long cpl_vector_get_size(const cpl_vector * v) { return v->n; } void cpl_vector_set(cpl_vector * v, cpl_size i, double val) { v->d[i] = val; } double cpl_vector_get(cpl_vector * v, cpl_size i) { return v->d[i]; } void cpl_vector_multiply(cpl_vector * v1, const cpl_vector * v2) { long i; assert(v1->n == v2->n); for (i = 0; i < v1->n; i++) { v1->d[i] *= v2->d[i]; } } void cpl_vector_power(cpl_vector * v, double power) { long i; for (i = 0; i < v->n; i++) { v->d[i] = pow(v->d[i], power); } } /*----------------------------------------------------------------------------- Private function prototypes -----------------------------------------------------------------------------*/ cpl_error_code cpl_matrix_product(cpl_matrix * self, const cpl_matrix * ma, const cpl_matrix * mb); cpl_matrix * cpl_matrix_product_normal_create(const cpl_matrix * self); cpl_error_code cpl_matrix_solve_spd(cpl_matrix *self, cpl_matrix *rhs); inline static void swap_rows(cpl_matrix *, cpl_size, cpl_size) CPL_ATTR_NONNULL; static void read_row(const cpl_matrix *, double *, cpl_size) CPL_ATTR_NONNULL; static void write_row(cpl_matrix *, const double *, cpl_size) CPL_ATTR_NONNULL; static void write_read_row(cpl_matrix *, double *, cpl_size) CPL_ATTR_NONNULL; static void read_column(const cpl_matrix *, double *, cpl_size) CPL_ATTR_NONNULL; static void write_column(cpl_matrix *, const double *, cpl_size) CPL_ATTR_NONNULL; static void write_read_column(cpl_matrix *, double *, cpl_size) CPL_ATTR_NONNULL; static cpl_error_code cpl_matrix_set_size_(cpl_matrix *, cpl_size, cpl_size); /*----------------------------------------------------------------------------- Function codes -----------------------------------------------------------------------------*/ /* * Private methods: */ /*----------------------------------------------------------------------------*/ /** @internal @brief Set the size of a matrix while destroying its elements @param self The matrix to modify @param nr New number of matrix rows. @param nc New number of matrix columns. @note Any pointer returned by cpl_matrix_get_data_const() may be invalidated */ /*----------------------------------------------------------------------------*/ static cpl_error_code cpl_matrix_set_size_(cpl_matrix * self, cpl_size nr, cpl_size nc) { cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT); if (self->nr != nr || self->nc != nc) { cpl_ensure_code(nr > 0, CPL_ERROR_ILLEGAL_INPUT); cpl_ensure_code(nc > 0, CPL_ERROR_ILLEGAL_INPUT); /* Need to resize the matrix */ if (self->nr * self->nc != nr * nc) { cpl_free(self->m); self->m = (double*)cpl_malloc((size_t)nr * (size_t)nc * sizeof(double)); } self->nr = nr; self->nc = nc; } return CPL_ERROR_NONE; } /*----------------------------------------------------------------------------*/ /** @internal @brief Swap two (different) rows @param self The matrix @param row1 One row @param row2 Another row @see cpl_matrix_swap_rows() @note No error checking done here Optimized due to repeated usage in cpl_matrix_decomp_lu(): 1) Removed redundant error checking - this allows the compiler to make 'swap' a register variable. 2) Use a single index for loop control and adressing the two rows - this redues the instruction count. This can have an effect when the swap is not memory bound. */ /*----------------------------------------------------------------------------*/ inline static void swap_rows(cpl_matrix * self, cpl_size row1, cpl_size row2) { double swap; cpl_size ncol = self->nc; double * pos1 = self->m + ncol * row1; double * pos2 = self->m + ncol * row2; while (ncol--) { swap = pos1[ncol]; pos1[ncol] = pos2[ncol]; pos2[ncol] = swap; } } /* * @internal * @brief * Just read a matrix row into the passed buffer. * * @param matrix Matrix where to read the row from. * @param pos Row number. * @param row Allocated buffer. * * @return Nothing. * * This private function reads the content of a matrix row into an allocated * buffer. No checks are performed. */ static void read_row(const cpl_matrix *matrix, double *row, cpl_size pos) { cpl_size i; for (i = 0, pos *= matrix->nc; i < matrix->nc; i++, pos++) row[i] = matrix->m[pos]; } /* * @internal * @brief * Just write into a matrix row the values in buffer. * * @param matrix Matrix where to write the buffer to. * @param pos Row number. * @param row Allocated buffer. * * @return Nothing. * * This private function writes the content of a buffer into a matrix row. * No checks are performed. */ static void write_row(cpl_matrix *matrix, const double *row, cpl_size pos) { cpl_size i; for (i = 0, pos *= matrix->nc; i < matrix->nc; i++, pos++) matrix->m[pos] = *row++; } /* * @internal * @brief * Just swap values in buffer with values in a matrix row. * * @param matrix Matrix to access. * @param pos Row to access. * @param row Allocated buffer. * * @return Nothing. * * This private function exchanges the values in buffer with the values in * a chosen matrix row. No checks are performed. */ static void write_read_row(cpl_matrix *matrix, double *row, cpl_size pos) { cpl_size i; double swap; for (i = 0, pos *= matrix->nc; i < matrix->nc; i++, pos++) { swap = matrix->m[pos]; matrix->m[pos] = row[i]; row[i] = swap; } } /* * @internal * @brief * Just read a matrix column into the passed buffer. * * @param matrix Matrix where to read the column from. * @param pos Column number. * @param column Allocated buffer. * * @return Nothing. * * This private function reads the content of a matrix column into an * allocated buffer. No checks are performed. */ static void read_column(const cpl_matrix *matrix, double *column, cpl_size pos) { cpl_size i; for (i = 0; i < matrix->nr; i++, pos += matrix->nc) column[i] = matrix->m[pos]; } /* * @internal * @brief * Just write into a matrix column the values in buffer. * * @param matrix Matrix where to write the buffer to. * @param pos Column number. * @param column Allocated buffer. * * @return Nothing. * * This private function writes the content of a buffer into a matrix column. * No checks are performed. */ static void write_column(cpl_matrix *matrix, const double *column, cpl_size pos) { cpl_size i; cpl_size size = matrix->nr * matrix->nc; for (i = pos; i < size; i += matrix->nc) matrix->m[i] = *column++; } /* * @internal * @brief * Just swap values in buffer with values in a matrix column. * * @param matrix Matrix to access. * @param pos Column to access. * @param column Allocated buffer. * * @return Nothing. * * This private function exchanges the values in buffer with the values in * a chosen matrix column. No checks are performed. */ static void write_read_column(cpl_matrix *matrix, double *column, cpl_size pos) { cpl_size i; double swap; for (i = 0; i < matrix->nr; i++, pos += matrix->nc) { swap = matrix->m[pos]; matrix->m[pos] = column[i]; column[i] = swap; } } /** * @brief * Create a zero matrix of given size. * * @param rows Number of matrix rows. * @param columns Number of matrix columns. * * @return Pointer to new matrix, or @c NULL in case of error. * * @error * * * * * *
CPL_ERROR_ILLEGAL_INPUT * rows or columns are not positive numbers. *
* @enderror * * This function allocates and initialises to zero a matrix of given size. * To destroy this matrix the function @c cpl_matrix_delete() should be used. */ cpl_matrix *cpl_matrix_new(cpl_size rows, cpl_size columns) { cpl_matrix *matrix; if (rows < 1) { (void)cpl_error_set_(CPL_ERROR_ILLEGAL_INPUT); return NULL; } if (columns < 1) { (void)cpl_error_set_(CPL_ERROR_ILLEGAL_INPUT); return NULL; } matrix = (cpl_matrix *)cpl_malloc(sizeof(cpl_matrix)); matrix->m = (double *)cpl_calloc((size_t)rows * (size_t)columns, sizeof(double)); matrix->nr = rows; matrix->nc = columns; return matrix; } /** * @brief * Create a new matrix from existing data. * * @param data Existing data buffer. * @param rows Number of matrix rows. * @param columns Number of matrix columns. * * @return Pointer to new matrix, or @c NULL in case of error. * * @error * * * * * * * * * *
CPL_ERROR_NULL_INPUT * The input data is a NULL pointer. *
CPL_ERROR_ILLEGAL_INPUT * rows or columns are not positive numbers. *
* @enderror * * This function creates a new matrix that will encapsulate the given * data. At any error condition, a @c NULL pointer would be returned. * Note that the size of the input data array is not checked in any way, * and it is expected to match the specified matrix sizes. The input * array is supposed to contain in sequence all the new @em cpl_matrix * rows. For instance, in the case of a 3x4 matrix, the input array * should contain 12 elements * @code * 0 1 2 3 4 5 6 7 8 9 10 11 * @endcode * that would correspond to the matrix elements * @code * 0 1 2 3 * 4 5 6 7 * 8 9 10 11 * @endcode * The data buffer is not copied, so it should not be deallocated while * the matrix is still in use: the function @c cpl_matrix_delete() would * take care of deallocating it. To avoid problems with the memory managment, * the specified data buffer should be allocated using the functions of * the @c cpl_memory module, and also statically allocated data should be * strictly avoided. If this were not the case, this matrix should not be * destroyed using @c cpl_matrix_delete(), but @c cpl_matrix_unwrap() * should be used instead; moreover, functions implying memory handling * (as @c cpl_matrix_set_size(), or @c cpl_matrix_delete_row() ) should not * be used. */ cpl_matrix *cpl_matrix_wrap(cpl_size rows, cpl_size columns, double *data) { cpl_matrix *matrix; if (rows < 1 || columns < 1) { cpl_error_set_(CPL_ERROR_ILLEGAL_INPUT); return NULL; } if (data == NULL) { cpl_error_set_(CPL_ERROR_NULL_INPUT); return NULL; } matrix = (cpl_matrix *)cpl_malloc(sizeof(cpl_matrix)); matrix->m = data; matrix->nr = rows; matrix->nc = columns; return matrix; } /** * @brief * Delete a matrix. * * @param matrix Pointer to a matrix to be deleted. * * @return Nothing. * * This function frees all the memory associated to a matrix. * If @em matrix is @c NULL, nothing is done. */ void cpl_matrix_delete(cpl_matrix *matrix) { if (matrix) { cpl_free(matrix->m); cpl_free(matrix); } } /** * @brief * Delete a matrix, but not its data buffer. * * @param matrix Pointer to a matrix to be deleted. * * @return Pointer to the internal data buffer. * * This function deallocates all the memory associated to a matrix, * with the exception of its data buffer. This type of destructor * should be used on matrices created with the @c cpl_matrix_wrap() * constructor, if the data buffer specified then was not allocated * using the functions of the @c cpl_memory module. In such a case, the * data buffer should be deallocated separately. See the documentation * of the function @c cpl_matrix_wrap(). If @em matrix is @c NULL, * nothing is done, and a @c NULL pointer is returned. */ void *cpl_matrix_unwrap(cpl_matrix *matrix) { void *data = NULL; if (matrix) { data = matrix->m; cpl_free(matrix); } return data; } /** * @brief * Get the number of rows of a matrix. * * @param matrix Pointer to the matrix to examine. * * @return Number of matrix rows, or zero in case of failure. * * @error * * * * * *
CPL_ERROR_NULL_INPUT * The input matrix is a NULL pointer. *
* @enderror * * Determine the number of rows in a matrix. */ inline cpl_size cpl_matrix_get_nrow(const cpl_matrix *matrix) { if (matrix == NULL) { /* inline precludes using cpl_error_set_() due to its use of __func__ */ (void) cpl_error_set("cpl_matrix_get_nrow", CPL_ERROR_NULL_INPUT); return 0; } return matrix->nr; } /** * @brief * Get the number of columns of a matrix. * * @param matrix Pointer to the matrix to examine. * * @return Number of matrix columns, or zero in case of failure. * * @error * * * * * *
CPL_ERROR_NULL_INPUT * The input matrix is a NULL pointer. *
* @enderror * * Determine the number of columns in a matrix. */ inline cpl_size cpl_matrix_get_ncol(const cpl_matrix *matrix) { if (matrix == NULL) { /* inline precludes using cpl_error_set_() due to its use of __func__ */ (void) cpl_error_set("cpl_matrix_get_ncol", CPL_ERROR_NULL_INPUT); return 0; } return matrix->nc; } /** * @brief * Get the pointer to a matrix data buffer, or @c NULL in case of error. * * @param matrix Input matrix. * * @return Pointer to the matrix data buffer. * * @error * * * * * *
CPL_ERROR_NULL_INPUT * The input matrix is a NULL pointer. *
* @enderror * * A @em cpl_matrix object includes an array of values of type @em double. * This function returns a pointer to this internal array, whose first * element corresponds to the @em cpl_matrix element 0,0. The internal * array contains in sequence all the @em cpl_matrix rows. For instance, * in the case of a 3x4 matrix, the array elements * @code * 0 1 2 3 4 5 6 7 8 9 10 11 * @endcode * would correspond to the following matrix elements: * @code * 0 1 2 3 * 4 5 6 7 * 8 9 10 11 * @endcode * * @note * Use at your own risk: direct manipulation of matrix data rules out * any check performed by the matrix object interface, and may introduce * inconsistencies between the information maintained internally, and * the actual matrix data and structure. */ inline double *cpl_matrix_get_data(cpl_matrix *matrix) { if (matrix == NULL) { /* inline precludes using cpl_error_set_() due to its use of __func__ */ (void) cpl_error_set("cpl_matrix_get_data", CPL_ERROR_NULL_INPUT); return NULL; } return matrix->m; } /** * @brief * Get the pointer to a matrix data buffer, or @c NULL in case of error. * * @param matrix Input matrix. * * @return Pointer to the matrix data buffer. * * @see cpl_matrix_get_data */ inline const double *cpl_matrix_get_data_const(const cpl_matrix *matrix) { if (matrix == NULL) { /* inline precludes using cpl_error_set_() due to its use of __func__ */ (void) cpl_error_set("cpl_matrix_get_data_const", CPL_ERROR_NULL_INPUT); return NULL; } return matrix->m; } /** * @brief * Get the value of a matrix element. * * @param matrix Pointer to a matrix. * @param row Matrix element row. * @param column Matrix element column. * * @return Value of the specified matrix element, or 0.0 in case of error. * * @error * * * * * * * * * *
CPL_ERROR_NULL_INPUT * The input matrix is a NULL pointer. *
CPL_ERROR_ACCESS_OUT_OF_RANGE * The accessed element is beyond the matrix boundaries. *
* @enderror * * Get the value of a matrix element. The matrix rows and columns are * counted from 0,0. */ double cpl_matrix_get(const cpl_matrix *matrix, cpl_size row, cpl_size column) { if (matrix == NULL) { (void)cpl_error_set_(CPL_ERROR_NULL_INPUT); return 0.0; } if (row < 0 || row >= matrix->nr || column < 0 || column >= matrix->nc) { (void)cpl_error_set_(CPL_ERROR_ACCESS_OUT_OF_RANGE); return 0.0; } return matrix->m[column + row * matrix->nc]; } /** * @brief * Write a value to a matrix element. * * @param matrix Input matrix. * @param row Matrix element row. * @param column Matrix element column. * @param value Value to write. * * @return @c CPL_ERROR_NONE on success. * * @error * * * * * * * * * *
CPL_ERROR_NULL_INPUT * The input matrix is a NULL pointer. *
CPL_ERROR_ACCESS_OUT_OF_RANGE * The accessed element is beyond the matrix boundaries. *
* @enderror * * Write a value to a matrix element. The matrix rows and columns are * counted from 0,0. */ inline cpl_error_code cpl_matrix_set(cpl_matrix *matrix, cpl_size row, cpl_size column, double value) { if (matrix == NULL) return cpl_error_set("cpl_matrix_set", CPL_ERROR_NULL_INPUT); if (row < 0 || row >= matrix->nr || column < 0 || column >= matrix->nc) return cpl_error_set("cpl_matrix_set", CPL_ERROR_ACCESS_OUT_OF_RANGE); matrix->m[column + row * matrix->nc] = value; return CPL_ERROR_NONE; } /** * @brief * Make a copy of a matrix. * * @param matrix Matrix to be duplicated. * * @return Pointer to the new matrix, or @c NULL in case of error. * * @error * * * * * *
CPL_ERROR_NULL_INPUT * The input matrix is a NULL pointer. *
* @enderror * * A copy of the input matrix is created. To destroy the duplicated matrix * the function @c cpl_matrix_delete() should be used. */ cpl_matrix *cpl_matrix_duplicate(const cpl_matrix *matrix) { cpl_matrix *new_matrix = NULL; if (matrix) { const cpl_size size = matrix->nr * matrix->nc; new_matrix = (cpl_matrix *)cpl_malloc(sizeof(cpl_matrix)); new_matrix->nr = matrix->nr; new_matrix->nc = matrix->nc; new_matrix->m = (double *)cpl_malloc((size_t)size * sizeof(double)); memcpy(new_matrix->m, matrix->m, (size_t)size * sizeof(double)); } else (void)cpl_error_set_(CPL_ERROR_NULL_INPUT); return new_matrix; } /** * @brief * Write the same value to all matrix elements. * * @param matrix Pointer to the matrix to access * @param value Value to write * * @return @c CPL_ERROR_NONE on success. * * @error * * * * * *
CPL_ERROR_NULL_INPUT * The input matrix is a NULL pointer. *
* @enderror * * Write the same value to all matrix elements. */ cpl_error_code cpl_matrix_fill(cpl_matrix *matrix, double value) { cpl_size size; double *m; if (matrix == NULL) return cpl_error_set_(CPL_ERROR_NULL_INPUT); size = matrix->nr * matrix->nc; m = matrix->m; while (size--) *m++ = value; return CPL_ERROR_NONE; } /** * @brief * Write the same value to a matrix row. * * @param matrix Matrix to access * @param value Value to write * @param row Sequence number of row to overwrite. * * @return @c CPL_ERROR_NONE on success. * * @error * * * * * * * * * *
CPL_ERROR_NULL_INPUT * The input matrix is a NULL pointer. *
CPL_ERROR_ACCESS_OUT_OF_RANGE * The specified row is outside the matrix boundaries. *
* @enderror * * Write the same value to a matrix row. Rows are counted starting from 0. */ cpl_error_code cpl_matrix_fill_row(cpl_matrix *matrix, double value, cpl_size row) { double *m; cpl_size count; if (matrix == NULL) return cpl_error_set_(CPL_ERROR_NULL_INPUT); if (row < 0 || row >= matrix->nr) return cpl_error_set_(CPL_ERROR_ACCESS_OUT_OF_RANGE); m = matrix->m + row * matrix->nc; count = matrix->nc; while (count--) *m++ = value; return CPL_ERROR_NONE; } /** * @brief * Write the same value to a matrix column. * * @param matrix Pointer to the matrix to access * @param value Value to write * @param column Sequence number of column to overwrite * * @return @c CPL_ERROR_NONE on success. * * @error * * * * * * * * * *
CPL_ERROR_NULL_INPUT * The input matrix is a NULL pointer. *
CPL_ERROR_ACCESS_OUT_OF_RANGE * The specified column is outside the matrix boundaries. *
* @enderror * * Write the same value to a matrix column. Columns are counted starting * from 0. */ cpl_error_code cpl_matrix_fill_column(cpl_matrix *matrix, double value, cpl_size column) { cpl_size count; double *m; if (matrix == NULL) return cpl_error_set_(CPL_ERROR_NULL_INPUT); if (column < 0 || column >= matrix->nc) return cpl_error_set_(CPL_ERROR_ACCESS_OUT_OF_RANGE); count = matrix->nr; m = matrix->m + column; while (count--) { *m = value; m += matrix->nc; } return CPL_ERROR_NONE; } /** * @brief * Write the values of a matrix into another matrix. * * @param matrix Pointer to matrix to be modified. * @param submatrix Pointer to matrix to get the values from. * @param row Position of row 0 of @em submatrix in @em matrix. * @param col Position of column 0 of @em submatrix in @em matrix. * * @return @c CPL_ERROR_NONE on success. * * @error * * * * * * * * * *
CPL_ERROR_NULL_INPUT * The input matrix or submatrix are NULL * pointers. *
CPL_ERROR_ACCESS_OUT_OF_RANGE * No overlap exists between the two matrices. *
* @enderror * * The values of @em submatrix are written to @em matrix starting at the * indicated row and column. There are no restrictions on the sizes of * @em submatrix: just the parts of @em submatrix overlapping @em matrix * are copied. There are no restrictions on @em row and @em col either, * that can also be negative. If the two matrices do not overlap, nothing * is done, but an error condition is set. */ cpl_error_code cpl_matrix_copy(cpl_matrix *matrix, const cpl_matrix *submatrix, cpl_size row, cpl_size col) { cpl_size endrow; cpl_size endcol; cpl_size subrow; cpl_size subcol; cpl_size r, c, sr, sc; double *m; double *sm; if (matrix == NULL || submatrix == NULL) return cpl_error_set_(CPL_ERROR_NULL_INPUT); endrow = row + submatrix->nr; endcol = col + submatrix->nc; /* * Check whether matrices overlap. */ if (row >= matrix->nr || endrow < 1 || col >= matrix->nc || endcol < 1) return cpl_error_set_(CPL_ERROR_ACCESS_OUT_OF_RANGE); /* * Define the overlap region on both matrices reference system: */ if (row < 0) { subrow = -row; row = 0; } else subrow = 0; if (col < 0) { subcol = -col; col = 0; } else subcol = 0; if (endrow > matrix->nr) endrow = matrix->nr; if (endcol > matrix->nc) endcol = matrix->nc; for (r = row, sr = subrow; r < endrow; r++, sr++) { m = matrix->m + r * matrix->nc + col; sm = submatrix->m + sr * submatrix->nc + subcol; for (c = col, sc = subcol; c < endcol; c++, sc++) *m++ = *sm++; } return CPL_ERROR_NONE; } /** * @brief * Write the same value into a submatrix of a matrix. * * @param matrix Pointer to matrix to be modified. * @param value Value to write. * @param row Start row of matrix submatrix. * @param col Start column of matrix submatrix. * @param nrow Number of rows of matrix submatrix. * @param ncol Number of columns of matrix submatrix. * * @return @c CPL_ERROR_NONE on success. * * @error * * * * * * * * * * * * * *
CPL_ERROR_NULL_INPUT * The input matrix is a NULL pointer. *
CPL_ERROR_ACCESS_OUT_OF_RANGE * The specified start position is outside the matrix * boundaries. *
CPL_ERROR_ILLEGAL_INPUT * nrow or ncol are not positive. *
* @enderror * * The specified value is written to @em matrix starting at the indicated * row and column; @em nrow and @em ncol can exceed the input matrix * boundaries, just the range overlapping the @em matrix is used in that * case. */ cpl_error_code cpl_matrix_fill_window(cpl_matrix *matrix, double value, cpl_size row, cpl_size col, cpl_size nrow, cpl_size ncol) { cpl_size endrow; cpl_size endcol; cpl_size r, c; double *m; if (matrix == NULL) return cpl_error_set_(CPL_ERROR_NULL_INPUT); if (row < 0 || row >= matrix->nr || col < 0 || col >= matrix->nc) return cpl_error_set_(CPL_ERROR_ACCESS_OUT_OF_RANGE); if (nrow < 1 || ncol < 1) return cpl_error_set_(CPL_ERROR_ILLEGAL_INPUT); endrow = row + nrow; if (endrow > matrix->nr) endrow = matrix->nr; endcol = col + ncol; if (endcol > matrix->nc) endcol = matrix->nc; for (r = row; r < endrow; r++) { m = matrix->m + r * matrix->nc + col; for (c = col; c < endcol; c++) *m++ = value; } return CPL_ERROR_NONE; } /** * @brief * Swap two matrix columns. * * @param matrix Pointer to matrix to be modified. * @param column1 One matrix column. * @param column2 Another matrix column. * * @return @c CPL_ERROR_NONE on success. * * @error * * * * * * * * * *
CPL_ERROR_NULL_INPUT * The input matrix is a NULL pointer. *
CPL_ERROR_ACCESS_OUT_OF_RANGE * Any of the specified columns is outside the matrix * boundaries. *
* @enderror * * The values of two given matrix columns are exchanged. Columns are * counted starting from 0. If the same column number is given twice, * nothing is done and no error is set. */ cpl_error_code cpl_matrix_swap_columns(cpl_matrix *matrix, cpl_size column1, cpl_size column2) { double swap; cpl_size nrow; if (matrix == NULL) return cpl_error_set_(CPL_ERROR_NULL_INPUT); if (column1 < 0 || column1 >= matrix->nc || column2 < 0 || column2 >= matrix->nc) return cpl_error_set_(CPL_ERROR_ACCESS_OUT_OF_RANGE); if (column1 == column2) return CPL_ERROR_NONE; nrow = matrix->nr; while (nrow--) { swap = matrix->m[column1]; matrix->m[column1] = matrix->m[column2]; matrix->m[column2] = swap; column1 += matrix->nc; column2 += matrix->nc; } return CPL_ERROR_NONE; } /** * @brief * Swap a matrix column with a matrix row. * * @param matrix Pointer to matrix to be modified. * @param row Matrix row. * * @return @c CPL_ERROR_NONE on success. * * @error * * * * * * * * * * * * * *
CPL_ERROR_NULL_INPUT * The input matrix is a NULL pointer. *
CPL_ERROR_ACCESS_OUT_OF_RANGE * The specified row is outside the matrix boundaries. *
CPL_ERROR_ILLEGAL_INPUT * The input matrix is not square. *
* @enderror * * The values of the indicated row are exchanged with the column having * the same sequence number. Rows and columns are counted starting from 0. */ cpl_error_code cpl_matrix_swap_rowcolumn(cpl_matrix *matrix, cpl_size row) { double swap; cpl_size i; cpl_size posr, posc; if (matrix == NULL) return cpl_error_set_(CPL_ERROR_NULL_INPUT); if (matrix->nr != matrix->nc) return cpl_error_set_(CPL_ERROR_ILLEGAL_INPUT); if (row < 0 || row >= matrix->nr) return cpl_error_set_(CPL_ERROR_ACCESS_OUT_OF_RANGE); posr = row; posc = row * matrix->nc; for (i = 0; i < matrix->nr; i++) { swap = matrix->m[posr]; matrix->m[posr] = matrix->m[posc]; matrix->m[posc] = swap; posr += matrix->nc; posc++; } return CPL_ERROR_NONE; } /** * @brief * Reverse order of rows in matrix. * * @param matrix Pointer to matrix to be reversed. * * @return @c CPL_ERROR_NONE on success. * * @error * * * * * *
CPL_ERROR_NULL_INPUT * The input matrix is a NULL pointer. *
* @enderror * * The order of the rows in the matrix is reversed in place. */ cpl_error_code cpl_matrix_flip_rows(cpl_matrix *matrix) { cpl_size i, j; if (matrix == NULL) return cpl_error_set_(CPL_ERROR_NULL_INPUT); for (i = 0, j = matrix->nr - 1; i < j; i++, j--) swap_rows(matrix, i, j); return CPL_ERROR_NONE; } /** * @brief * Reverse order of columns in matrix. * * @param matrix Pointer to matrix to be reversed. * * @return @c CPL_ERROR_NONE on success. * * @error * * * * * *
CPL_ERROR_NULL_INPUT * The input matrix is a NULL pointer. *
* @enderror * * The order of the columns in the matrix is reversed in place. */ cpl_error_code cpl_matrix_flip_columns(cpl_matrix *matrix) { cpl_size i, j; if (matrix == NULL) return cpl_error_set_(CPL_ERROR_NULL_INPUT); for (i = 0, j = matrix->nc - 1; i < j; i++, j--) cpl_matrix_swap_columns(matrix, i, j); return CPL_ERROR_NONE; } /** * @brief * Create transposed matrix. * * @param matrix Pointer to matrix to be transposed. * * @return Pointer to transposed matrix. If a @c NULL pointer is passed, * a @c NULL pointer is returned. * * @error * * * * * *
CPL_ERROR_NULL_INPUT * The input matrix is a NULL pointer. *
* @enderror * * The transposed of the input matrix is created. To destroy the new matrix * the function @c cpl_matrix_delete() should be used. */ cpl_matrix *cpl_matrix_transpose_create(const cpl_matrix *matrix) { cpl_matrix *transposed = NULL; cpl_size i, j; double *m; double *tm; if (matrix == NULL) { cpl_error_set_(CPL_ERROR_NULL_INPUT); return NULL; } tm = (double *)cpl_malloc((size_t)matrix->nc * (size_t)matrix->nr * sizeof(*tm)); transposed = cpl_matrix_wrap(matrix->nc, matrix->nr, tm); m = matrix->m; for (i = 0; i < matrix->nr; i++) for (j = 0, tm = transposed->m + i; j < matrix->nc; j++, tm += matrix->nr) *tm = *m++; return transposed; } /** * @brief * Reframe a matrix. * * @param matrix Pointer to matrix to be modified. * @param top Extra rows on top. * @param bottom Extra rows on bottom. * @param left Extra columns on left. * @param right Extra columns on right. * * @return @c CPL_ERROR_NONE on success. * * @error * * * * * * * * * *
CPL_ERROR_NULL_INPUT * The input matrix is a NULL pointer. *
CPL_ERROR_ILLEGAL_OUTPUT * Attempt to shrink matrix to zero size (or less). *
* @enderror * * The input matrix is reframed according to specifications. Extra rows * and column on the sides might also be negative, as long as they are * compatible with the matrix sizes: the input matrix would be reduced * in size accordingly, but an attempt to remove all matrix columns * and/or rows is flagged as an error because zero length matrices are * illegal. The old matrix elements contained in the new shape are left * unchanged, and new matrix elements added by the reshaping are initialised * to zero. No reshaping (i.e., all the extra rows set to zero) would * not be flagged as an error. * * @note * The pointer to the matrix data buffer may change, therefore * pointers previously retrieved by calling @c cpl_matrix_get_data() * should be discarded. */ cpl_error_code cpl_matrix_resize(cpl_matrix *matrix, cpl_size top, cpl_size bottom, cpl_size left, cpl_size right) { cpl_matrix *resized; cpl_size nr, nc; if (matrix == NULL) return cpl_error_set_(CPL_ERROR_NULL_INPUT); if (top == 0 && bottom == 0 && left == 0 && right == 0) return CPL_ERROR_NONE; nr = matrix->nr + top + bottom; nc = matrix->nc + left + right; if (nr < 1 || nc < 1) return cpl_error_set_(CPL_ERROR_ILLEGAL_OUTPUT); resized = cpl_matrix_new(nr, nc); cpl_matrix_copy(resized, matrix, top, left); cpl_free(matrix->m); matrix->m = cpl_matrix_unwrap(resized); matrix->nr = nr; matrix->nc = nc; return CPL_ERROR_NONE; } /** * @brief * Resize a matrix. * * @param matrix Pointer to matrix to be resized. * @param rows New number of rows. * @param columns New number of columns. * * @return @c CPL_ERROR_NONE on success. * * @error * * * * * * * * * *
CPL_ERROR_NULL_INPUT * The input matrix is a NULL pointer. *
CPL_ERROR_ILLEGAL_OUTPUT * Attempt to shrink matrix to zero size (or less). *
* @enderror * * The input matrix is resized according to specifications. The old * matrix elements contained in the resized matrix are left unchanged, * and new matrix elements added by an increase of the matrix number of * rows and/or columns are initialised to zero. * * @note * The pointer to the matrix data buffer may change, therefore * pointers previously retrieved by calling @c cpl_matrix_get_data() * should be discarded. */ cpl_error_code cpl_matrix_set_size(cpl_matrix *matrix, cpl_size rows, cpl_size columns) { return cpl_matrix_resize(matrix, 0, rows - matrix->nr, 0, columns - matrix->nc) ? cpl_error_set_where_() : CPL_ERROR_NONE; } /** * @brief * Rows-by-columns product of two matrices. * * @param matrix1 Pointer to left side matrix. * @param matrix2 Pointer to right side matrix. * * @return Pointer to product matrix, or @c NULL in case of error. * * @error * * * * * * * * * *
CPL_ERROR_NULL_INPUT * An input matrix is a NULL pointer. *
CPL_ERROR_INCOMPATIBLE_INPUT * The number of columns of the first matrix is not equal to * the number of rows of the second matrix. *
* @enderror * * Rows-by-columns product of two matrices. The number of columns of the * first matrix must be equal to the number of rows of the second matrix. * To destroy the new matrix the function @c cpl_matrix_delete() should * be used. */ cpl_matrix *cpl_matrix_product_create(const cpl_matrix *matrix1, const cpl_matrix *matrix2) { cpl_matrix *self; cpl_ensure(matrix1 != NULL, CPL_ERROR_NULL_INPUT, NULL); cpl_ensure(matrix2 != NULL, CPL_ERROR_NULL_INPUT, NULL); cpl_ensure(matrix1->nc == matrix2->nr, CPL_ERROR_INCOMPATIBLE_INPUT, NULL); /* Create data-buffer without overhead of initializing */ self = cpl_matrix_wrap(matrix1->nr, matrix2->nc, cpl_calloc((size_t)matrix1->nr * (size_t)matrix2->nc , sizeof(*self))); (void) cpl_matrix_product(self, matrix1, matrix2); return self; } /* * @brief Replace a matrix by its LU-decomposition * @param self n X n non-singular matrix to decompose * @param perm n-integer array to be filled with the row permutations * @param psig On success set to 1/-1 for an even/odd number of permutations * @return CPL_ERROR_NONE on success, or the relevant CPL error code * @note If the matrix is singular the elements of self become undefined * @see Golub & Van Loan, Matrix Computations, Algorithms 3.2.1 (Outer Product * Gaussian Elimination) and 3.4.1 (Gauss Elimination with Partial Pivoting). * * @error * * * * * * * * * * * * * * * * * * * * * *
CPL_ERROR_NULL_INPUT * An input pointer is NULL. *
CPL_ERROR_ILLEGAL_INPUT * self is not an n by n matrix. *
CPL_ERROR_INCOMPATIBLE_INPUT * self and perm have incompatible sizes. *
CPL_ERROR_TYPE_MISMATCH * perm is not of type CPL_TYPE_INT. *
CPL_ERROR_SINGULAR_MATRIX * self is singular. *
* @enderror * */ cpl_error_code cpl_matrix_decomp_lu(cpl_matrix * self, int * iperm, int * psig) { const cpl_size n = cpl_matrix_get_ncol(self); int nn; cpl_size i, j; const double * aread; double * awrite; cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT); cpl_ensure_code(iperm != NULL, CPL_ERROR_NULL_INPUT); cpl_ensure_code(psig != NULL, CPL_ERROR_NULL_INPUT); cpl_ensure_code(self->nc == self->nr, CPL_ERROR_ILLEGAL_INPUT); nn = (int)n; cpl_ensure_code((cpl_size)nn == n, CPL_ERROR_ILLEGAL_INPUT); aread = awrite = self->m; /* No row permutations yet */ *psig = 1; for (i = 0; i < n; i++) { iperm[i] = i; } for (j = 0; j < n - 1; j++) { /* Find maximum in the j-th column */ double pivot = fabs(aread[j + j * n]); cpl_size ipivot = j; for (i = j + 1; i < n; i++) { const double aij = fabs(aread[j + i * n]); if (aij > pivot) { pivot = aij; ipivot = i; } } if (pivot <= 0.0) { return CPL_ERROR_SINGULAR_MATRIX; } if (ipivot > j) { /* The maximum is below the diagonal - swap rows */ const int iswap = iperm[j]; iperm[j] = iperm[ipivot]; iperm[ipivot] = iswap; *psig = - (*psig); swap_rows(self, j, ipivot); } pivot = aread[j + j * n]; for (i = j + 1; i < n; i++) { const double aij = aread[j + i * n] / pivot; cpl_size k; awrite[j + i * n] = aij; for (k = j + 1; k < n; k++) { const double ajk = aread[k + j * n]; const double aik = aread[k + i * n]; awrite[k + i * n] = aik - aij * ajk; } } } /* Check if A(n,n) is non-zero */ return fabs(aread[n * (n-1) + (n-1)]) > 0.0 ? CPL_ERROR_NONE : CPL_ERROR_SINGULAR_MATRIX; } /* * @brief Solve a LU-system * @param self n X n LU-matrix from cpl_matrix_decomp_lu() * @param rhs m right-hand-sides to be replaced by their solution * @param perm n-integer array filled with the row permutations, or NULL * @return CPL_ERROR_NONE on success, or the relevant CPL error code * @see cpl_matrix_decomp_lu() * * @error * * * * * * * * * * * * * * * * * * * * * *
CPL_ERROR_NULL_INPUT * An input pointer is NULL. *
CPL_ERROR_ILLEGAL_INPUT * self is not an n by n matrix. *
CPL_ERROR_INCOMPATIBLE_INPUT * The array or matrices not have the same number of rows. *
CPL_ERROR_TYPE_MISMATCH * perm is non-NULL and not of type CPL_TYPE_INT. *
CPL_ERROR_DIVISION_BY_ZERO * The main diagonal of U contains a zero. This error can only occur * if the LU-matrix does not come from a successful call to * cpl_matrix_decomp_lu(). *
* @enderror * */ cpl_error_code cpl_matrix_solve_lu(const cpl_matrix * self, cpl_matrix * rhs, const int * iperm) { cpl_size n, i, j, k; double * x = NULL; /* Avoid false uninit warning */ const double * aread; const double * bread; double * bwrite; cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT); cpl_ensure_code(rhs != NULL, CPL_ERROR_NULL_INPUT); cpl_ensure_code(self->nc == self->nr, CPL_ERROR_ILLEGAL_INPUT); cpl_ensure_code(rhs->nr == self->nr, CPL_ERROR_INCOMPATIBLE_INPUT); n = self->nc; aread = self->m; bread = bwrite = rhs->m; if (iperm == NULL) { return CPL_ERROR_NULL_INPUT; } else { x = (double*) cpl_malloc((size_t)n * sizeof(*x)); } for (k=0; k < rhs->nc; k++) { if (iperm != NULL) { /* Un-permute the rows in column k */ for (i = 0; i < n; i++) { x[i] = bread[rhs->nc * i + k]; } for (i = 0; i < n; i++) { bwrite[rhs->nc * i + k] = x[iperm[i]]; } } /* Forward substitution in column k */ for (i = 1; i < n; i++) { double tmp = bread[rhs->nc * i + k]; for (j = 0; j < i; j++) { const double aij = aread[n * i + j]; tmp -= aij * bread[rhs->nc * j + k]; } bwrite[rhs->nc * i + k] = tmp; } /* Back substitution in column k */ for (i = n - 1; i >= 0; i--) { double tmp = bread[rhs->nc * i + k]; for (j = i + 1; j < n; j++) { const double aij = aread[n * i + j]; tmp -= aij * bread[rhs->nc * j + k]; } /* Check for a bug in the calling function */ if (aread[n * i + i] == 0.0) break; bwrite[rhs->nc * i + k] = tmp/aread[n * i + i]; } if (i >= 0) break; } return k == rhs->nc ? CPL_ERROR_NONE : cpl_error_set_(CPL_ERROR_DIVISION_BY_ZERO); } /*----------------------------------------------------------------------------*/ /** @brief Replace a matrix by its Cholesky-decomposition, L * transpose(L) = A @param self N by N symmetric positive-definite matrix to decompose @return CPL_ERROR_NONE on success, or the relevant CPL error code @note Only the upper triangle of self is read, L is written in the lower triangle @note If the matrix is singular the elements of self become undefined @see Golub & Van Loan, Matrix Computations, Algorithm 4.2.1 (Cholesky: Gaxpy Version). @error
CPL_ERROR_NULL_INPUT An input pointer is NULL.
CPL_ERROR_ILLEGAL_INPUT self is not an n by n matrix.
CPL_ERROR_SINGULAR_MATRIX self is not symmetric, positive definite.
@enderror */ /*----------------------------------------------------------------------------*/ cpl_error_code cpl_matrix_decomp_chol(cpl_matrix * self) { const cpl_size n = cpl_matrix_get_ncol(self); double * awrite = cpl_matrix_get_data(self); const double * aread = awrite; double sub = 0.0; /* Fix (false) uninit warning */ cpl_size i, j = 0; /* Fix (false) uninit warning */ cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT); cpl_ensure_code(cpl_matrix_get_nrow(self) == n, CPL_ERROR_ILLEGAL_INPUT); for (i = 0; i < n; i++) { for (j = i; j < n; j++) { cpl_size k; sub = aread[n * i + j]; for (k = i-1; k >= 0; k--) { sub -= aread[n * i + k] * aread[n * j + k]; } if (j > i) { awrite[n * j + i] = sub/aread[n * i + i]; } else if (sub > 0.0) { awrite[n * i + i] = sqrt(sub); } else { break; } } if (j < n) break; } return i == n ? CPL_ERROR_NONE : CPL_ERROR_SINGULAR_MATRIX; } /*----------------------------------------------------------------------------*/ /** @brief Solve a L*transpose(L)-system @param self N by N L*transpose(L)-matrix from cpl_matrix_decomp_chol() @param rhs M right-hand-sides to be replaced by their solution @return CPL_ERROR_NONE on success, or the relevant CPL error code @see cpl_matrix_decomp_chol() @note Only the lower triangle of self is accessed @error
CPL_ERROR_NULL_INPUT An input pointer is NULL.
CPL_ERROR_ILLEGAL_INPUT self is not an n by n matrix.
CPL_ERROR_INCOMPATIBLE_INPUT The specified matrices do not have the same number of rows.
CPL_ERROR_DIVISION_BY_ZERO The main diagonal of L contains a zero. This error can only occur if the L*transpose(L)-matrix does not come from a successful call to cpl_matrix_decomp_chol().
@enderror */ /*----------------------------------------------------------------------------*/ cpl_error_code cpl_matrix_solve_chol(const cpl_matrix * self, cpl_matrix * rhs) { const cpl_size n = cpl_matrix_get_ncol(self); const cpl_size nrhs = cpl_matrix_get_ncol(rhs); cpl_size i, j, k; const double * aread; const double * bread; double * bwrite; cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT); cpl_ensure_code(rhs != NULL, CPL_ERROR_NULL_INPUT); cpl_ensure_code(cpl_matrix_get_nrow(self) == n, CPL_ERROR_ILLEGAL_INPUT); cpl_ensure_code(cpl_matrix_get_nrow(rhs) == n, CPL_ERROR_INCOMPATIBLE_INPUT); aread = cpl_matrix_get_data_const(self); bread = bwrite = cpl_matrix_get_data(rhs); for (k=0; k < nrhs; k++) { /* Forward substitution in column k */ for (i = 0; i < n; i++) { double sub = bread[nrhs * i + k]; for (j = i-1; j >= 0; j--) { sub -= aread[n * i + j] * bread[nrhs * j + k]; } cpl_ensure_code(aread[n * i + i] != 0.0, CPL_ERROR_DIVISION_BY_ZERO); bwrite[nrhs * i + k] = sub/aread[n * i + i]; } /* Back substitution in column k */ for (i = n-1; i >= 0; i--) { double sub = bread[nrhs * i + k]; for (j = i+1; j < n; j++) { sub -= aread[n * j + i] * bread[nrhs * j + k]; } bwrite[nrhs * i + k] = sub/aread[n * i + i]; } } return CPL_ERROR_NONE; } /** * @brief * Solution of a linear system. * * @param coeff A non-singular N by N matrix. * @param rhs A matrix containing one or more right-hand-sides. * @note rhs must have N rows and may contain more than one column, * which each represent an independent right-hand-side. * * @return A newly allocated solution matrix with the size as rhs, * or @c NULL on error. * * @error * * * * * * * * * * * * * * * * * *
CPL_ERROR_NULL_INPUT * Any input is a NULL pointer. *
CPL_ERROR_ILLEGAL_INPUT * coeff is not a square matrix. *
CPL_ERROR_INCOMPATIBLE_INPUT * coeff and rhs do not have the same number of rows. *
CPL_ERROR_SINGULAR_MATRIX * coeff is singular (to working precision). *
* @enderror * * Compute the solution of a system of N equations with N unknowns: * * coeff * X = rhs * * @em coeff must be an NxN matrix, and @em rhs a NxM matrix. * M greater than 1 means that multiple independent right-hand-sides * are solved for. * To destroy the solution matrix the function @c cpl_matrix_delete() * should be used. * */ cpl_matrix *cpl_matrix_solve(const cpl_matrix *coeff, const cpl_matrix *rhs) { cpl_matrix * lu; cpl_matrix * x; int * iperm; cpl_size n; int sig; cpl_error_code error; cpl_ensure(coeff != NULL, CPL_ERROR_NULL_INPUT, NULL); cpl_ensure(rhs != NULL, CPL_ERROR_NULL_INPUT, NULL); n = coeff->nc; cpl_ensure(coeff->nr == n, CPL_ERROR_ILLEGAL_INPUT, NULL); cpl_ensure(rhs->nr == n, CPL_ERROR_INCOMPATIBLE_INPUT, NULL); lu = cpl_matrix_duplicate(coeff); iperm = (int*) cpl_malloc((size_t)n * sizeof(*iperm)); if (cpl_matrix_decomp_lu(lu, iperm, &sig)) { cpl_matrix_delete(lu); cpl_free(iperm); (void)cpl_error_set_where_(); return NULL; } x = cpl_matrix_duplicate(rhs); /* should not be able to fail at this point */ error = cpl_matrix_solve_lu(lu, x, iperm); cpl_matrix_delete(lu); cpl_free(iperm); if (error) { cpl_matrix_delete(x); x = NULL; (void)cpl_error_set_where_(); } return x; } /** * @brief * Solution of overdetermined linear equations in a least squares sense. * * @param coeff The N by M matrix of coefficients, where N >= M. * @param rhs An N by K matrix containing K right-hand-sides. * @note rhs may contain more than one column, * which each represent an independent right-hand-side. * * @return A newly allocated M by K solution matrix, * or @c NULL on error. * * @error * * * * * * * * * * * * * *
CPL_ERROR_NULL_INPUT * Any input is a NULL pointer. *
CPL_ERROR_INCOMPATIBLE_INPUT * coeff and rhs do not have the same number of rows. *
CPL_ERROR_SINGULAR_MATRIX * The matrix is (near) singular and a solution cannot be computed. *
* @enderror * * The following linear system of N equations and M unknowns * is given: * * @code * coeff * X = rhs * @endcode * * where @em coeff is the NxM matrix of the coefficients, @em X is the * MxK matrix of the unknowns, and @em rhs the NxK matrix containing * the K right hand side(s). * * The solution to the normal equations is known to be a least-squares * solution, i.e. the 2-norm of coeff * X - rhs is minimized by the * solution to * transpose(coeff) * coeff * X = transpose(coeff) * rhs. * * In the case that coeff is square (N is equal to M) it gives a faster * and more accurate result to use cpl_matrix_solve(). * * The solution matrix should be deallocated with the function * @c cpl_matrix_delete(). */ cpl_matrix *cpl_matrix_solve_normal(const cpl_matrix *coeff, const cpl_matrix *rhs) { cpl_matrix * solution; cpl_matrix * At; cpl_matrix * AtA; cpl_ensure(coeff != NULL, CPL_ERROR_NULL_INPUT, NULL); cpl_ensure(rhs != NULL, CPL_ERROR_NULL_INPUT, NULL); cpl_ensure(rhs->nr == coeff->nr, CPL_ERROR_INCOMPATIBLE_INPUT, NULL); At = cpl_matrix_transpose_create(coeff); solution = cpl_matrix_product_create(At, rhs); AtA = cpl_matrix_product_normal_create(At); cpl_matrix_delete(At); if (cpl_matrix_solve_spd(AtA, solution)) { cpl_matrix_delete(solution); solution = NULL; (void)cpl_error_set_where_(); } cpl_matrix_delete(AtA); return solution; } /*----------------------------------------------------------------------------*/ /** @internal @brief Solution of Symmetric, Positive Definite system of linear equations. * @param self The N by N Symmetric, Positive Definite matrix. @param rhs An N by K matrix containing K right-hand-sides. @note rhs may contain more than one column, which each represent an independent right-hand-side. The solution is written in rhs. Only the upper triangular part of self is read, while the lower triangular part is modified. @return CPL_ERROR_NONE on success, or the relevant CPL error code * @error
CPL_ERROR_NULL_INPUT self or rhs is a NULL pointer.
CPL_ERROR_INCOMPATIBLE_INPUT self and rhs do not have the same number of rows.
CPL_ERROR_ILLEGAL_INPUT self is not an N by N matrix.
CPL_ERROR_SINGULAR_MATRIX self is (near) singular and a solution cannot be computed.
@enderror */ /*----------------------------------------------------------------------------*/ cpl_error_code cpl_matrix_solve_spd(cpl_matrix *self, cpl_matrix *rhs) { cpl_ensure_code(!cpl_matrix_decomp_chol(self), cpl_error_get_code()); cpl_ensure_code(!cpl_matrix_solve_chol(self, rhs), cpl_error_get_code()); return CPL_ERROR_NONE; } /** * @internal * @brief Compute A = B * transpose(B) * * @param self Pre-allocated M x M matrix to hold the result * @param other M x N Matrix to multiply with its transpose @return CPL_ERROR_NONE or the relevant CPL error code on error * @note Only the upper triangle of A is computed, while the elements * below the main diagonal have undefined values. * @see cpl_matrix_product_create() * * @error * * * * * * * * * * * * * * * *
CPL_ERROR_NULL_INPUT * An input matrix is a NULL pointer. *
CPL_ERROR_ILLEGAL_INPUT * self is not an M by M matrix. *
CPL_ERROR_INCOMPATIBLE_INPUT * The two matrices have a different number of rows. *
* @enderror * */ cpl_error_code cpl_matrix_product_normal(cpl_matrix * self, const cpl_matrix * other) { double sum; const double * ai = cpl_matrix_get_data_const(other); const double * aj; double * bwrite = cpl_matrix_get_data(self); const cpl_size m = cpl_matrix_get_nrow(self); const cpl_size n = cpl_matrix_get_ncol(other); cpl_size i, j, k; cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT); cpl_ensure_code(other != NULL, CPL_ERROR_NULL_INPUT); cpl_ensure_code(cpl_matrix_get_ncol(self) == m, CPL_ERROR_ILLEGAL_INPUT); cpl_ensure_code(cpl_matrix_get_nrow(other) == m, CPL_ERROR_INCOMPATIBLE_INPUT); #ifdef CPL_MATRIX_PRODUCT_NORMAL_RESET /* Initialize all values to zero. This is done to avoid access of uninitilized memory in case someone passes the matrix to for example cpl_matrix_dump(). */ (void)memset(bwrite, 0, m * n * sizeof(*bwrite)); #endif /* The result at (i,j) is the dot-product of i'th and j'th row */ for (i = 0; i < m; i++, bwrite += m, ai += n) { aj = ai; /* aj points to first entry in j'th row */ for (j = i; j < m; j++, aj += n) { sum = 0.0; for (k = 0; k < n; k++) { sum += ai[k] * aj[k]; } bwrite[j] = sum; } } return CPL_ERROR_NONE; } /** * @internal * @brief Create and compute A = B * transpose(B) * * @param self M x N Matrix * @return Pointer to created M x M product matrix, or @c NULL on error. * @note Only the upper triangle of A is computed, while the elements * below the main diagonal have undefined values. * @see cpl_matrix_product_create() * * @error * * * * * *
CPL_ERROR_NULL_INPUT * An input matrix is a NULL pointer. *
* @enderror * * To destroy the new matrix the function @c cpl_matrix_delete() should * be used. */ cpl_matrix * cpl_matrix_product_normal_create(const cpl_matrix * self) { const size_t m = cpl_matrix_get_nrow(self); cpl_matrix * product = cpl_matrix_wrap((cpl_size)m, (cpl_size)m, cpl_malloc(m * m * sizeof(double))); if (cpl_matrix_product_normal(product, self)) { cpl_matrix_delete(product); product = NULL; (void)cpl_error_set_where_(); } return product; } /*----------------------------------------------------------------------------*/ /** @internal @brief Fill a matrix with the product of A * B @param self The matrix to fill, is or else will be set to size M x N @param ma The matrix A, of size M x K @param mb The matrix B, of size K * N @return CPL_ERROR_NONE or the relevant CPL error code on error @note If upon entry, self has a size m *n, which differs from its size on return, then the result of preceeding calls to cpl_matrix_get_data() are invalid. @see cpl_matrix_product_create() */ /*----------------------------------------------------------------------------*/ cpl_error_code cpl_matrix_product(cpl_matrix * self, const cpl_matrix * ma, const cpl_matrix * mb) { const size_t bs = 48; double * ds; const double * d1 = cpl_matrix_get_data_const(ma); const double * d2 = cpl_matrix_get_data_const(mb); const size_t nr = cpl_matrix_get_nrow(ma); const size_t nc = cpl_matrix_get_ncol(mb); const size_t nk = cpl_matrix_get_nrow(mb); size_t i, j, k, ib, jb, kb; cpl_ensure_code(ma != NULL, CPL_ERROR_NULL_INPUT); cpl_ensure_code(mb != NULL, CPL_ERROR_NULL_INPUT); cpl_ensure_code((size_t)ma->nc == nk, CPL_ERROR_INCOMPATIBLE_INPUT); if (cpl_matrix_set_size_(self, nr, nc)) return cpl_error_set_where_(); ds = cpl_matrix_get_data(self); /* simple blocking to reduce cache misses */ for (i = 0; i < nr; i += bs) { for (j = 0; j < nc; j += bs) { for (k = 0; k < nk; k += bs) { for (ib = i; ib < CX_MIN(i + bs, nr); ib++) { for (jb = j; jb < CX_MIN(j + bs, nc); jb++) { double sum = 0.; for (kb = k; kb < CX_MIN(k + bs, nk); kb++) { sum += d1[ib * nk + kb] * d2[kb * nc + jb]; } ds[ib * nc + jb] += sum; } } } } } return CPL_ERROR_NONE; } /*----------------------------------------------------------------------------*/ /** @internal @brief Fill a matrix with the product of A * B' @param self The matrix to fill, is or else will be set to size M x N @param ma The matrix A, of size M x K @param mb The matrix B, of size N x K @return CPL_ERROR_NONE or the relevant CPL error code on error @note The use of the transpose of B causes a more efficient memory access @note Changing the order of A and B is allowed, it transposes the result @see cpl_matrix_product_create() */ /*----------------------------------------------------------------------------*/ cpl_error_code cpl_matrix_product_transpose(cpl_matrix * self, const cpl_matrix * ma, const cpl_matrix * mb) { double sum; double * ds; const double * d1 = cpl_matrix_get_data_const(ma); const double * d2 = cpl_matrix_get_data_const(mb); const double * di; const cpl_size nr = cpl_matrix_get_nrow(ma); const cpl_size nc = cpl_matrix_get_nrow(mb); const cpl_size nk = cpl_matrix_get_ncol(mb); cpl_size i, j, k; cpl_ensure_code(ma != NULL, CPL_ERROR_NULL_INPUT); cpl_ensure_code(mb != NULL, CPL_ERROR_NULL_INPUT); cpl_ensure_code(ma->nc == nk, CPL_ERROR_INCOMPATIBLE_INPUT); if (cpl_matrix_set_size_(self, nr, nc)) return cpl_error_set_where_(); ds = cpl_matrix_get_data(self); for (i = 0; i < nr; i++, d1 += nk) { /* Since ma and mb are addressed in the same manner, they can use the same index, k */ di = d2; /* di points to first entry in i'th row */ for (j = 0; j < nc; j++, di += nk) { sum = 0.0; for (k = 0; k < nk; k++) { sum += d1[k] * di[k]; } ds[nc * i + j] = sum; } } return CPL_ERROR_NONE; } /*----------------------------------------------------------------------------*/ /** @internal @brief Fill a matrix with the product of B * A * B' @param self The matrix to fill, is or else will be set to size N x N @param ma The matrix A, of size M x M @param mb The matrix B, of size N x M @return CPL_ERROR_NONE or the relevant CPL error code on error @note Requires 2 * N * M * (N + M) FLOPs and M doubles of temporary storage @see cpl_matrix_product_create() The intermediate result, C = A * B', is computed one column at a time, each column of C is then used once to compute one row of the final result. The performed sequence of floating point operations is exactly the same as in the combination of the two calls cpl_matrix_product_transpose(C, A, B); cpl_matrix_product_create(B, C); and the result is therefore also exactly identical. The difference lies in the reduced use of temporary storage and in the unit stride access, both of which lead to better performance (for large matrices). */ /*----------------------------------------------------------------------------*/ cpl_error_code cpl_matrix_product_bilinear(cpl_matrix * self, const cpl_matrix * ma, const cpl_matrix * mb) { double * ds; double * cj; /* Holds one column of A * B' */ const double * d1 = cpl_matrix_get_data_const(ma); const double * d2 = cpl_matrix_get_data_const(mb); const cpl_size nr = cpl_matrix_get_nrow(mb); const cpl_size nc = cpl_matrix_get_ncol(mb); cpl_size i, j, k; cpl_ensure_code(ma != NULL, CPL_ERROR_NULL_INPUT); cpl_ensure_code(mb != NULL, CPL_ERROR_NULL_INPUT); cpl_ensure_code(ma->nr == nc, CPL_ERROR_INCOMPATIBLE_INPUT); cpl_ensure_code(ma->nc == nc, CPL_ERROR_INCOMPATIBLE_INPUT); if (cpl_matrix_set_size_(self, nr, nr)) return cpl_error_set_where_(); ds = cpl_matrix_get_data(self); cj = cpl_malloc((size_t)nc * sizeof(*cj)); for (j = 0; j < nr; j++) { /* First compute the jth column of C = A * B', whose kth element is the dot-product of kth row of A and the jth row of B, both of which are read with unit stride. */ for (k = 0; k < nc; k++) { double sum = 0.0; for (i = 0; i < nc; i++) { sum += d1[nc * k + i] * d2[nc * j + i]; } cj[k] = sum; } /* Then compute (i, j)th element of the result, which is the dot-product of the ith row of B and the jth column of C, both of which are read with unit stride. */ for (i = 0; i < nr; i++) { double sum = 0.0; for (k = 0; k < nc; k++) { sum += d2[nc * i + k] * cj[k]; } ds[nr * i + j] = sum; } } cpl_free(cj); return CPL_ERROR_NONE; } /*----------------------------------------------------------------------------*/ /** @internal @brief Solve a L*transpose(L)-system with a transposed Right Hand Side @param self N by N L*transpose(L)-matrix from cpl_matrix_decomp_chol() @param rhs M right-hand-sides to be replaced by their solution @return CPL_ERROR_NONE on success, or the relevant CPL error code @see cpl_matrix_solve_chol() @note Only the lower triangle of self is accessed @note The use of the transpose of rhs causes a more efficient memory access @error
CPL_ERROR_NULL_INPUT An input pointer is NULL.
CPL_ERROR_ILLEGAL_INPUT self is not an n by n matrix.
CPL_ERROR_INCOMPATIBLE_INPUT Selfs number of rows differs from rhs' number of columns.
CPL_ERROR_DIVISION_BY_ZERO The main diagonal of L contains a zero. This error can only occur if the L*transpose(L)-matrix does not come from a successful call to cpl_matrix_decomp_chol().
@enderror */ /*----------------------------------------------------------------------------*/ cpl_error_code cpl_matrix_solve_chol_transpose(const cpl_matrix * self, cpl_matrix * rhs) { const cpl_size n = cpl_matrix_get_ncol(self); const cpl_size nrhs = cpl_matrix_get_nrow(rhs); cpl_size i, j, k; const double * aread; const double * ai; double * bk; double sub; cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT); cpl_ensure_code(rhs != NULL, CPL_ERROR_NULL_INPUT); cpl_ensure_code(self->nr == n, CPL_ERROR_ILLEGAL_INPUT); cpl_ensure_code(rhs->nc == n, CPL_ERROR_INCOMPATIBLE_INPUT); aread = self->m; /* bk points to first entry in k'th right hand side */ bk = rhs->m; for (k=0; k < nrhs; k++, bk += n) { /* Forward substitution in column k */ /* Since self and rhs are addressed in the same manner, they can use the same index, j */ ai = aread; /* ai points to first entry in i'th row */ for (i = 0; i < n; i++, ai += n) { sub = 0.0; for (j = 0; j < i; j++) { sub += ai[j] * bk[j]; } cpl_ensure_code(k > 0 || ai[j] != 0.0, CPL_ERROR_DIVISION_BY_ZERO); bk[j] = (bk[j] - sub) / ai[j]; } /* Back substitution in column k */ for (i = n-1; i >= 0; i--) { sub = bk[i]; for (j = i+1; j < n; j++) { sub -= aread[n * j + i] * bk[j]; } bk[i] = sub/aread[n * i + i]; } } return CPL_ERROR_NONE; } /* ---------------------------------------------------------------------------*/ /** * @brief generic 1d vandermonde matrix * * @param sample sampling positions * @param degree degree of polynomial * @param func function evaluating polynomials from [0, degree] at * sampling point * @param offset offset of func (1 for indexing starting with 1) * @return matrix containing the vandermonde matrix */ /* ---------------------------------------------------------------------------*/ cpl_matrix * vander1d( const cpl_vector * sample, cpl_size degree, void (*func)(double, double *, int), size_t offset) { size_t i; const size_t nr = cpl_vector_get_size(sample); const size_t nc = degree + 1; cpl_matrix * V = cpl_matrix_new(nr, nc); double * v = cpl_matrix_get_data(V); const double * d = sample->d; for (i = 0; i < nr; i++) { if (offset != 0) { double tmp[nc + offset]; func(d[i], tmp, nc); memcpy(&v[i * nc], &tmp[offset], nc * sizeof(*v)); } else { func(d[i], &v[i * nc], nc); } } return V; } cpl_matrix * vander2d( const cpl_vector * sample_x, const cpl_vector * sample_y, cpl_size degree, void (*func)(double, double, double *, int), size_t offset) { size_t i; const size_t nr = cpl_vector_get_size(sample_x); const size_t nc = degree + 1; cpl_matrix * V = cpl_matrix_new(nr, nc); double * v = cpl_matrix_get_data(V); const double * dx = sample_x->d; const double * dy = sample_y->d; assert(cpl_vector_get_size(sample_y) == nr); for (i = 0; i < nr; i++) { if (offset != 0) { double tmp[nc + offset]; func(dx[i], dy[i], tmp, nc); memcpy(&v[i * nc], &tmp[offset], nc * sizeof(*v)); } else { func(dx[i], dy[i], &v[i * nc], nc); } } return V; } static void polynomial(double x, double * p, int ncoefs) { int i; p[0] = 1.; for (i = 1; i < ncoefs; i++) { p[i] = pow(x, i); } } cpl_matrix * polyvander1d( const cpl_vector * sample, cpl_size degree) { return vander1d(sample, degree, &polynomial, 0); } void lsqfit( const cpl_matrix * design, const cpl_vector * values, const cpl_vector * errors, cpl_matrix **coef) { /* weight response and design */ cpl_vector * vrhs = cpl_vector_duplicate(errors); cpl_vector_power(vrhs, -1); cpl_matrix * wdesign = cpl_matrix_duplicate(design); long i, j; for (i = 0; i < cpl_vector_get_size(errors); i++) { double w = vrhs->d[i]; for (j = 0; j < cpl_matrix_get_ncol(wdesign); j++) { cpl_matrix_set(wdesign, i, j, cpl_matrix_get(wdesign, i, j) * w); } } cpl_vector_multiply(vrhs, values); cpl_matrix * rhs = cpl_matrix_wrap(cpl_vector_get_size(vrhs), 1, vrhs->d); /* solve Ax = b */ /* cpl_matrix_solve_normal(design, rhs) + covariance */ { cpl_matrix * At = cpl_matrix_transpose_create(wdesign); cpl_matrix * AtA = cpl_matrix_product_normal_create(At); /* RRt = AtA */ cpl_matrix_decomp_chol(AtA); /* solve for pseudo inverse: (RRt)P=At*/ cpl_matrix_solve_chol(AtA, At); /* compute solution to system Ax=b -> x=Pb */ *coef = cpl_matrix_product_create(At, rhs); /* compute covariance matrix cov(b) = PPt */ /* cpl_matrix * cov = cpl_matrix_new(cpl_matrix_get_ncol(At), */ /* cpl_matrix_get_ncol(At)); */ /* cpl_matrix_product_transpose(cov, At, At); */ cpl_matrix_delete(At); cpl_matrix_delete(AtA); } cpl_matrix_unwrap(rhs); cpl_vector_delete(vrhs); cpl_matrix_delete(wdesign); return; } void lsqfit_nr( double x[], double y[], double sig[], int ndat, double a[], int ma, void (*funcs)(double, double [], int)) { int i; cpl_vector * vX = cpl_vector_wrap(ndat, &x[1]); cpl_vector * vY = cpl_vector_wrap(ndat, &y[1]); cpl_vector * vS; cpl_matrix * design = vander1d(vX, ma - 1, funcs, 1); cpl_matrix * mcoef; if (sig) { vS = cpl_vector_wrap(ndat, &sig[1]); } else { vS = cpl_vector_new(ndat); for (i= 0; i < ndat; i++) cpl_vector_set(vS, i, 1.); } lsqfit(design, vY, vS, &mcoef); for (i = 1; i <= ma; i++) { a[i] = cpl_matrix_get(mcoef, i - 1, 0); } cpl_vector_unwrap(vX); cpl_vector_unwrap(vY); if (sig) { cpl_vector_unwrap(vS); } else { cpl_vector_delete(vS); } cpl_matrix_delete(design); cpl_matrix_delete(mcoef); } void lsqfit2d_nr( double x[], double y[], double z[], double sig[], int ndat, double a[], int ma, void (*funcs)(double, double, double [], int)) { int i; cpl_vector * vX = cpl_vector_wrap(ndat, &x[1]); cpl_vector * vY = cpl_vector_wrap(ndat, &y[1]); cpl_vector * vZ = cpl_vector_wrap(ndat, &z[1]); cpl_vector * vS; cpl_matrix * design = vander2d(vX, vY, ma - 1, funcs, 1); cpl_matrix * mcoef; if (sig) { vS = cpl_vector_wrap(ndat, &sig[1]); } else { vS = cpl_vector_new(ndat); for (i= 0; i < ndat; i++) cpl_vector_set(vS, i, 1.); } lsqfit(design, vZ, vS, &mcoef); for (i = 1; i <= ma; i++) { a[i] = cpl_matrix_get(mcoef, i - 1, 0); } cpl_vector_unwrap(vX); cpl_vector_unwrap(vY); cpl_vector_unwrap(vZ); if (sig) { cpl_vector_unwrap(vS); } else { cpl_vector_delete(vS); } cpl_matrix_delete(design); cpl_matrix_delete(mcoef); } cpl_vector * eval_poly( cpl_matrix * design, cpl_matrix * coef) { cpl_matrix * fvalues = cpl_matrix_product_create(design, coef); cpl_vector * res = cpl_vector_wrap(cpl_matrix_get_nrow(fvalues), cpl_matrix_get_data(fvalues)); cpl_matrix_unwrap(fvalues); return res; }