| 1 |
/* |
| 2 |
* This file is part of the ESO Common Pipeline Library |
| 3 |
* Copyright (C) 2001-2008,2014 European Southern Observatory |
| 4 |
* |
| 5 |
* This program is free software; you can redistribute it and/or modify |
| 6 |
* it under the terms of the GNU General Public License as published by |
| 7 |
* the Free Software Foundation; either version 2 of the License, or |
| 8 |
* (at your option) any later version. |
| 9 |
* |
| 10 |
* This program is distributed in the hope that it will be useful, |
| 11 |
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 12 |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 13 |
* GNU General Public License for more details. |
| 14 |
* |
| 15 |
* You should have received a copy of the GNU General Public License |
| 16 |
* along with this program; if not, write to the Free Software |
| 17 |
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA |
| 18 |
*/ |
| 19 |
|
| 20 |
/** |
| 21 |
* @defgroup cpl_matrix Matrices |
| 22 |
* |
| 23 |
* This module provides functions to create, destroy and use a @em cpl_matrix. |
| 24 |
* The elements of a @em cpl_matrix with M rows and N columns are counted |
| 25 |
* from 0,0 to M-1,N-1. The matrix element 0,0 is the one at the upper left |
| 26 |
* corner of a matrix. The CPL matrix functions work properly only in the |
| 27 |
* case the matrices elements do not contain garbage (such as @c NaN or |
| 28 |
* infinity). |
| 29 |
* |
| 30 |
* @par Synopsis: |
| 31 |
* @code |
| 32 |
* #include <cpl_matrix.h> |
| 33 |
* @endcode |
| 34 |
*/ |
| 35 |
|
| 36 |
#include "cpl_matrix.h" |
| 37 |
#include <stdlib.h> |
| 38 |
#include <string.h> |
| 39 |
#include <math.h> |
| 40 |
#include <assert.h> |
| 41 |
|
| 42 |
/**@{*/ |
| 43 |
|
| 44 |
#ifndef inline |
| 45 |
#define inline /* inline */ |
| 46 |
#endif |
| 47 |
|
| 48 |
#define dtiny(x, t) ((x) < 0.0 ? (x) >= -(t) : (x) <= (t)) |
| 49 |
|
| 50 |
#define CPL_ATTR_NONNULL |
| 51 |
|
| 52 |
typedef enum { |
| 53 |
CPL_ERROR_NONE = 0, |
| 54 |
CPL_ERROR_NULL_INPUT, |
| 55 |
CPL_ERROR_ILLEGAL_INPUT, |
| 56 |
CPL_ERROR_INCOMPATIBLE_INPUT, |
| 57 |
CPL_ERROR_ILLEGAL_OUTPUT, |
| 58 |
CPL_ERROR_ACCESS_OUT_OF_RANGE, |
| 59 |
CPL_ERROR_SINGULAR_MATRIX, |
| 60 |
CPL_ERROR_DIVISION_BY_ZERO, |
| 61 |
} cpl_error_code; |
| 62 |
|
| 63 |
|
| 64 |
/* |
| 65 |
* The cpl_matrix type: |
| 66 |
*/ |
| 67 |
|
| 68 |
struct _cpl_matrix { |
| 69 |
cpl_size nc; |
| 70 |
cpl_size nr; |
| 71 |
double *m; |
| 72 |
}; |
| 73 |
|
| 74 |
struct _cpl_vector { |
| 75 |
cpl_size n; |
| 76 |
double * d; |
| 77 |
} ; |
| 78 |
|
| 79 |
#define CX_MIN(a, b) (((a) < (b)) ? (a) : (b)) |
| 80 |
#define cpl_ensure_code(a, ret) \ |
| 81 |
do { \ |
| 82 |
if (!(a)) \ |
| 83 |
return ret; \ |
| 84 |
} while(0) |
| 85 |
|
| 86 |
#define cpl_ensure(a, code, ret) \ |
| 87 |
do { \ |
| 88 |
if (!(a)) \ |
| 89 |
return ret; \ |
| 90 |
} while(0) |
| 91 |
|
| 92 |
static void * cpl_malloc(size_t sz) |
| 93 |
{ |
| 94 |
return malloc(sz); |
| 95 |
} |
| 96 |
|
| 97 |
static void * cpl_calloc(size_t sz, size_t elsz) |
| 98 |
{ |
| 99 |
return calloc(sz, elsz); |
| 100 |
} |
| 101 |
|
| 102 |
static void cpl_free(void * p) |
| 103 |
{ |
| 104 |
free(p); |
| 105 |
} |
| 106 |
|
| 107 |
static cpl_error_code cpl_error_set_(cpl_error_code code) |
| 108 |
{ |
| 109 |
return code; |
| 110 |
} |
| 111 |
|
| 112 |
static cpl_error_code cpl_error_set(const char *a , cpl_error_code code) |
| 113 |
{ |
| 114 |
return code; |
| 115 |
} |
| 116 |
|
| 117 |
static void cpl_error_set_where(const char *a) |
| 118 |
{ |
| 119 |
} |
| 120 |
|
| 121 |
static cpl_error_code cpl_error_set_where_(void) |
| 122 |
{ |
| 123 |
return 0; |
| 124 |
} |
| 125 |
|
| 126 |
static cpl_error_code cpl_error_get_code(void) |
| 127 |
{ |
| 128 |
return 0; |
| 129 |
} |
| 130 |
|
| 131 |
/* minimal vector, no error checking */ |
| 132 |
cpl_vector * cpl_vector_new(cpl_size n) |
| 133 |
{ |
| 134 |
cpl_vector * v = cpl_malloc(sizeof(*v)); |
| 135 |
v->d = cpl_malloc(n * sizeof(v->d[0])); |
| 136 |
v->n = n; |
| 137 |
return v; |
| 138 |
} |
| 139 |
|
| 140 |
cpl_vector * cpl_vector_duplicate(const cpl_vector * a) |
| 141 |
{ |
| 142 |
cpl_vector *v = cpl_vector_new(a->n); |
| 143 |
memcpy(v->d, a->d, a->n * sizeof(a->d[0])); |
| 144 |
return v; |
| 145 |
} |
| 146 |
|
| 147 |
cpl_vector * cpl_vector_wrap(long n, double *d) |
| 148 |
{ |
| 149 |
cpl_vector *v = cpl_malloc(sizeof(*v)); |
| 150 |
v->n = n; |
| 151 |
v->d = d; |
| 152 |
return v; |
| 153 |
} |
| 154 |
|
| 155 |
void cpl_vector_delete(cpl_vector * v) |
| 156 |
{ |
| 157 |
if (v == NULL) |
| 158 |
return; |
| 159 |
cpl_free(v->d); |
| 160 |
cpl_free(v); |
| 161 |
} |
| 162 |
|
| 163 |
void cpl_vector_unwrap(cpl_vector * v) |
| 164 |
{ |
| 165 |
if (v == NULL) |
| 166 |
return; |
| 167 |
cpl_free(v); |
| 168 |
} |
| 169 |
|
| 170 |
long cpl_vector_get_size(const cpl_vector * v) |
| 171 |
{ |
| 172 |
return v->n; |
| 173 |
} |
| 174 |
|
| 175 |
void cpl_vector_set(cpl_vector * v, cpl_size i, double val) |
| 176 |
{ |
| 177 |
v->d[i] = val; |
| 178 |
} |
| 179 |
|
| 180 |
double cpl_vector_get(cpl_vector * v, cpl_size i) |
| 181 |
{ |
| 182 |
return v->d[i]; |
| 183 |
} |
| 184 |
|
| 185 |
void cpl_vector_multiply(cpl_vector * v1, const cpl_vector * v2) |
| 186 |
{ |
| 187 |
long i; |
| 188 |
assert(v1->n == v2->n); |
| 189 |
for (i = 0; i < v1->n; i++) { |
| 190 |
v1->d[i] *= v2->d[i]; |
| 191 |
} |
| 192 |
} |
| 193 |
|
| 194 |
void cpl_vector_power(cpl_vector * v, double power) |
| 195 |
{ |
| 196 |
long i; |
| 197 |
for (i = 0; i < v->n; i++) { |
| 198 |
v->d[i] = pow(v->d[i], power); |
| 199 |
} |
| 200 |
} |
| 201 |
|
| 202 |
/*----------------------------------------------------------------------------- |
| 203 |
Private function prototypes |
| 204 |
-----------------------------------------------------------------------------*/ |
| 205 |
|
| 206 |
cpl_error_code cpl_matrix_product(cpl_matrix * self, |
| 207 |
const cpl_matrix * ma, |
| 208 |
const cpl_matrix * mb); |
| 209 |
cpl_matrix * cpl_matrix_product_normal_create(const cpl_matrix * self); |
| 210 |
cpl_error_code cpl_matrix_solve_spd(cpl_matrix *self, |
| 211 |
cpl_matrix *rhs); |
| 212 |
|
| 213 |
inline |
| 214 |
static void swap_rows(cpl_matrix *, cpl_size, cpl_size) CPL_ATTR_NONNULL; |
| 215 |
static void read_row(const cpl_matrix *, double *, cpl_size) CPL_ATTR_NONNULL; |
| 216 |
static void write_row(cpl_matrix *, const double *, cpl_size) CPL_ATTR_NONNULL; |
| 217 |
static void write_read_row(cpl_matrix *, double *, cpl_size) CPL_ATTR_NONNULL; |
| 218 |
static |
| 219 |
void read_column(const cpl_matrix *, double *, cpl_size) CPL_ATTR_NONNULL; |
| 220 |
static |
| 221 |
void write_column(cpl_matrix *, const double *, cpl_size) CPL_ATTR_NONNULL; |
| 222 |
static |
| 223 |
void write_read_column(cpl_matrix *, double *, cpl_size) CPL_ATTR_NONNULL; |
| 224 |
static cpl_error_code cpl_matrix_set_size_(cpl_matrix *, cpl_size, cpl_size); |
| 225 |
|
| 226 |
/*----------------------------------------------------------------------------- |
| 227 |
Function codes |
| 228 |
-----------------------------------------------------------------------------*/ |
| 229 |
|
| 230 |
/* |
| 231 |
* Private methods: |
| 232 |
*/ |
| 233 |
|
| 234 |
/*----------------------------------------------------------------------------*/ |
| 235 |
/** |
| 236 |
@internal |
| 237 |
@brief Set the size of a matrix while destroying its elements |
| 238 |
@param self The matrix to modify |
| 239 |
@param nr New number of matrix rows. |
| 240 |
@param nc New number of matrix columns. |
| 241 |
@note Any pointer returned by cpl_matrix_get_data_const() may be invalidated |
| 242 |
|
| 243 |
*/ |
| 244 |
/*----------------------------------------------------------------------------*/ |
| 245 |
static |
| 246 |
cpl_error_code cpl_matrix_set_size_(cpl_matrix * self, cpl_size nr, cpl_size nc) |
| 247 |
{ |
| 248 |
|
| 249 |
cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT); |
| 250 |
|
| 251 |
if (self->nr != nr || self->nc != nc) { |
| 252 |
|
| 253 |
cpl_ensure_code(nr > 0, CPL_ERROR_ILLEGAL_INPUT); |
| 254 |
cpl_ensure_code(nc > 0, CPL_ERROR_ILLEGAL_INPUT); |
| 255 |
|
| 256 |
/* Need to resize the matrix */ |
| 257 |
|
| 258 |
if (self->nr * self->nc != nr * nc) { |
| 259 |
cpl_free(self->m); |
| 260 |
self->m = (double*)cpl_malloc((size_t)nr * (size_t)nc |
| 261 |
* sizeof(double)); |
| 262 |
} |
| 263 |
self->nr = nr; |
| 264 |
self->nc = nc; |
| 265 |
} |
| 266 |
|
| 267 |
return CPL_ERROR_NONE; |
| 268 |
} |
| 269 |
|
| 270 |
|
| 271 |
/*----------------------------------------------------------------------------*/ |
| 272 |
/** |
| 273 |
@internal |
| 274 |
@brief Swap two (different) rows |
| 275 |
@param self The matrix |
| 276 |
@param row1 One row |
| 277 |
@param row2 Another row |
| 278 |
@see cpl_matrix_swap_rows() |
| 279 |
@note No error checking done here |
| 280 |
|
| 281 |
Optimized due to repeated usage in cpl_matrix_decomp_lu(): |
| 282 |
1) Removed redundant error checking |
| 283 |
- this allows the compiler to make 'swap' a register variable. |
| 284 |
2) Use a single index for loop control and adressing the two rows |
| 285 |
- this redues the instruction count. |
| 286 |
|
| 287 |
This can have an effect when the swap is not memory bound. |
| 288 |
|
| 289 |
*/ |
| 290 |
/*----------------------------------------------------------------------------*/ |
| 291 |
inline |
| 292 |
static void swap_rows(cpl_matrix * self, cpl_size row1, cpl_size row2) |
| 293 |
{ |
| 294 |
double swap; |
| 295 |
cpl_size ncol = self->nc; |
| 296 |
double * pos1 = self->m + ncol * row1; |
| 297 |
double * pos2 = self->m + ncol * row2; |
| 298 |
|
| 299 |
while (ncol--) { |
| 300 |
swap = pos1[ncol]; |
| 301 |
pos1[ncol] = pos2[ncol]; |
| 302 |
pos2[ncol] = swap; |
| 303 |
} |
| 304 |
} |
| 305 |
|
| 306 |
|
| 307 |
/* |
| 308 |
* @internal |
| 309 |
* @brief |
| 310 |
* Just read a matrix row into the passed buffer. |
| 311 |
* |
| 312 |
* @param matrix Matrix where to read the row from. |
| 313 |
* @param pos Row number. |
| 314 |
* @param row Allocated buffer. |
| 315 |
* |
| 316 |
* @return Nothing. |
| 317 |
* |
| 318 |
* This private function reads the content of a matrix row into an allocated |
| 319 |
* buffer. No checks are performed. |
| 320 |
*/ |
| 321 |
static void read_row(const cpl_matrix *matrix, double *row, cpl_size pos) |
| 322 |
{ |
| 323 |
|
| 324 |
cpl_size i; |
| 325 |
|
| 326 |
for (i = 0, pos *= matrix->nc; i < matrix->nc; i++, pos++) |
| 327 |
row[i] = matrix->m[pos]; |
| 328 |
|
| 329 |
} |
| 330 |
|
| 331 |
/* |
| 332 |
* @internal |
| 333 |
* @brief |
| 334 |
* Just write into a matrix row the values in buffer. |
| 335 |
* |
| 336 |
* @param matrix Matrix where to write the buffer to. |
| 337 |
* @param pos Row number. |
| 338 |
* @param row Allocated buffer. |
| 339 |
* |
| 340 |
* @return Nothing. |
| 341 |
* |
| 342 |
* This private function writes the content of a buffer into a matrix row. |
| 343 |
* No checks are performed. |
| 344 |
*/ |
| 345 |
static void write_row(cpl_matrix *matrix, const double *row, cpl_size pos) |
| 346 |
{ |
| 347 |
|
| 348 |
cpl_size i; |
| 349 |
|
| 350 |
for (i = 0, pos *= matrix->nc; i < matrix->nc; i++, pos++) |
| 351 |
matrix->m[pos] = *row++; |
| 352 |
|
| 353 |
} |
| 354 |
|
| 355 |
/* |
| 356 |
* @internal |
| 357 |
* @brief |
| 358 |
* Just swap values in buffer with values in a matrix row. |
| 359 |
* |
| 360 |
* @param matrix Matrix to access. |
| 361 |
* @param pos Row to access. |
| 362 |
* @param row Allocated buffer. |
| 363 |
* |
| 364 |
* @return Nothing. |
| 365 |
* |
| 366 |
* This private function exchanges the values in buffer with the values in |
| 367 |
* a chosen matrix row. No checks are performed. |
| 368 |
*/ |
| 369 |
static void write_read_row(cpl_matrix *matrix, double *row, cpl_size pos) |
| 370 |
{ |
| 371 |
|
| 372 |
cpl_size i; |
| 373 |
double swap; |
| 374 |
|
| 375 |
for (i = 0, pos *= matrix->nc; i < matrix->nc; i++, pos++) { |
| 376 |
swap = matrix->m[pos]; |
| 377 |
matrix->m[pos] = row[i]; |
| 378 |
row[i] = swap; |
| 379 |
} |
| 380 |
|
| 381 |
} |
| 382 |
|
| 383 |
/* |
| 384 |
* @internal |
| 385 |
* @brief |
| 386 |
* Just read a matrix column into the passed buffer. |
| 387 |
* |
| 388 |
* @param matrix Matrix where to read the column from. |
| 389 |
* @param pos Column number. |
| 390 |
* @param column Allocated buffer. |
| 391 |
* |
| 392 |
* @return Nothing. |
| 393 |
* |
| 394 |
* This private function reads the content of a matrix column into an |
| 395 |
* allocated buffer. No checks are performed. |
| 396 |
*/ |
| 397 |
static void read_column(const cpl_matrix *matrix, double *column, cpl_size pos) |
| 398 |
{ |
| 399 |
|
| 400 |
cpl_size i; |
| 401 |
|
| 402 |
for (i = 0; i < matrix->nr; i++, pos += matrix->nc) |
| 403 |
column[i] = matrix->m[pos]; |
| 404 |
|
| 405 |
} |
| 406 |
|
| 407 |
/* |
| 408 |
* @internal |
| 409 |
* @brief |
| 410 |
* Just write into a matrix column the values in buffer. |
| 411 |
* |
| 412 |
* @param matrix Matrix where to write the buffer to. |
| 413 |
* @param pos Column number. |
| 414 |
* @param column Allocated buffer. |
| 415 |
* |
| 416 |
* @return Nothing. |
| 417 |
* |
| 418 |
* This private function writes the content of a buffer into a matrix column. |
| 419 |
* No checks are performed. |
| 420 |
*/ |
| 421 |
static void write_column(cpl_matrix *matrix, const double *column, cpl_size pos) |
| 422 |
{ |
| 423 |
|
| 424 |
cpl_size i; |
| 425 |
cpl_size size = matrix->nr * matrix->nc; |
| 426 |
|
| 427 |
|
| 428 |
for (i = pos; i < size; i += matrix->nc) |
| 429 |
matrix->m[i] = *column++; |
| 430 |
|
| 431 |
} |
| 432 |
|
| 433 |
/* |
| 434 |
* @internal |
| 435 |
* @brief |
| 436 |
* Just swap values in buffer with values in a matrix column. |
| 437 |
* |
| 438 |
* @param matrix Matrix to access. |
| 439 |
* @param pos Column to access. |
| 440 |
* @param column Allocated buffer. |
| 441 |
* |
| 442 |
* @return Nothing. |
| 443 |
* |
| 444 |
* This private function exchanges the values in buffer with the values in |
| 445 |
* a chosen matrix column. No checks are performed. |
| 446 |
*/ |
| 447 |
static void write_read_column(cpl_matrix *matrix, double *column, cpl_size pos) |
| 448 |
{ |
| 449 |
|
| 450 |
cpl_size i; |
| 451 |
double swap; |
| 452 |
|
| 453 |
for (i = 0; i < matrix->nr; i++, pos += matrix->nc) { |
| 454 |
swap = matrix->m[pos]; |
| 455 |
matrix->m[pos] = column[i]; |
| 456 |
column[i] = swap; |
| 457 |
} |
| 458 |
|
| 459 |
} |
| 460 |
|
| 461 |
/** |
| 462 |
* @brief |
| 463 |
* Create a zero matrix of given size. |
| 464 |
* |
| 465 |
* @param rows Number of matrix rows. |
| 466 |
* @param columns Number of matrix columns. |
| 467 |
* |
| 468 |
* @return Pointer to new matrix, or @c NULL in case of error. |
| 469 |
* |
| 470 |
* @error |
| 471 |
* <table class="ec" align="center"> |
| 472 |
* <tr> |
| 473 |
* <td class="ecl">CPL_ERROR_ILLEGAL_INPUT</td> |
| 474 |
* <td class="ecr"> |
| 475 |
* <i>rows</i> or <i>columns</i> are not positive numbers. |
| 476 |
* </td> |
| 477 |
* </tr> |
| 478 |
* </table> |
| 479 |
* @enderror |
| 480 |
* |
| 481 |
* This function allocates and initialises to zero a matrix of given size. |
| 482 |
* To destroy this matrix the function @c cpl_matrix_delete() should be used. |
| 483 |
*/ |
| 484 |
|
| 485 |
cpl_matrix *cpl_matrix_new(cpl_size rows, cpl_size columns) |
| 486 |
{ |
| 487 |
|
| 488 |
|
| 489 |
cpl_matrix *matrix; |
| 490 |
|
| 491 |
|
| 492 |
if (rows < 1) { |
| 493 |
(void)cpl_error_set_(CPL_ERROR_ILLEGAL_INPUT); |
| 494 |
return NULL; |
| 495 |
} |
| 496 |
|
| 497 |
if (columns < 1) { |
| 498 |
(void)cpl_error_set_(CPL_ERROR_ILLEGAL_INPUT); |
| 499 |
return NULL; |
| 500 |
} |
| 501 |
|
| 502 |
matrix = (cpl_matrix *)cpl_malloc(sizeof(cpl_matrix)); |
| 503 |
|
| 504 |
matrix->m = (double *)cpl_calloc((size_t)rows * (size_t)columns, |
| 505 |
sizeof(double)); |
| 506 |
matrix->nr = rows; |
| 507 |
matrix->nc = columns; |
| 508 |
|
| 509 |
return matrix; |
| 510 |
|
| 511 |
} |
| 512 |
|
| 513 |
|
| 514 |
/** |
| 515 |
* @brief |
| 516 |
* Create a new matrix from existing data. |
| 517 |
* |
| 518 |
* @param data Existing data buffer. |
| 519 |
* @param rows Number of matrix rows. |
| 520 |
* @param columns Number of matrix columns. |
| 521 |
* |
| 522 |
* @return Pointer to new matrix, or @c NULL in case of error. |
| 523 |
* |
| 524 |
* @error |
| 525 |
* <table class="ec" align="center"> |
| 526 |
* <tr> |
| 527 |
* <td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 528 |
* <td class="ecr"> |
| 529 |
* The input <i>data</i> is a <tt>NULL</tt> pointer. |
| 530 |
* </td> |
| 531 |
* </tr> |
| 532 |
* <tr> |
| 533 |
* <td class="ecl">CPL_ERROR_ILLEGAL_INPUT</td> |
| 534 |
* <td class="ecr"> |
| 535 |
* <i>rows</i> or <i>columns</i> are not positive numbers. |
| 536 |
* </td> |
| 537 |
* </tr> |
| 538 |
* </table> |
| 539 |
* @enderror |
| 540 |
* |
| 541 |
* This function creates a new matrix that will encapsulate the given |
| 542 |
* data. At any error condition, a @c NULL pointer would be returned. |
| 543 |
* Note that the size of the input data array is not checked in any way, |
| 544 |
* and it is expected to match the specified matrix sizes. The input |
| 545 |
* array is supposed to contain in sequence all the new @em cpl_matrix |
| 546 |
* rows. For instance, in the case of a 3x4 matrix, the input array |
| 547 |
* should contain 12 elements |
| 548 |
* @code |
| 549 |
* 0 1 2 3 4 5 6 7 8 9 10 11 |
| 550 |
* @endcode |
| 551 |
* that would correspond to the matrix elements |
| 552 |
* @code |
| 553 |
* 0 1 2 3 |
| 554 |
* 4 5 6 7 |
| 555 |
* 8 9 10 11 |
| 556 |
* @endcode |
| 557 |
* The data buffer is not copied, so it should not be deallocated while |
| 558 |
* the matrix is still in use: the function @c cpl_matrix_delete() would |
| 559 |
* take care of deallocating it. To avoid problems with the memory managment, |
| 560 |
* the specified data buffer should be allocated using the functions of |
| 561 |
* the @c cpl_memory module, and also statically allocated data should be |
| 562 |
* strictly avoided. If this were not the case, this matrix should not be |
| 563 |
* destroyed using @c cpl_matrix_delete(), but @c cpl_matrix_unwrap() |
| 564 |
* should be used instead; moreover, functions implying memory handling |
| 565 |
* (as @c cpl_matrix_set_size(), or @c cpl_matrix_delete_row() ) should not |
| 566 |
* be used. |
| 567 |
*/ |
| 568 |
|
| 569 |
cpl_matrix *cpl_matrix_wrap(cpl_size rows, cpl_size columns, double *data) |
| 570 |
{ |
| 571 |
|
| 572 |
|
| 573 |
cpl_matrix *matrix; |
| 574 |
|
| 575 |
|
| 576 |
if (rows < 1 || columns < 1) { |
| 577 |
cpl_error_set_(CPL_ERROR_ILLEGAL_INPUT); |
| 578 |
return NULL; |
| 579 |
} |
| 580 |
|
| 581 |
if (data == NULL) { |
| 582 |
cpl_error_set_(CPL_ERROR_NULL_INPUT); |
| 583 |
return NULL; |
| 584 |
} |
| 585 |
|
| 586 |
matrix = (cpl_matrix *)cpl_malloc(sizeof(cpl_matrix)); |
| 587 |
|
| 588 |
matrix->m = data; |
| 589 |
matrix->nr = rows; |
| 590 |
matrix->nc = columns; |
| 591 |
|
| 592 |
return matrix; |
| 593 |
|
| 594 |
} |
| 595 |
|
| 596 |
|
| 597 |
/** |
| 598 |
* @brief |
| 599 |
* Delete a matrix. |
| 600 |
* |
| 601 |
* @param matrix Pointer to a matrix to be deleted. |
| 602 |
* |
| 603 |
* @return Nothing. |
| 604 |
* |
| 605 |
* This function frees all the memory associated to a matrix. |
| 606 |
* If @em matrix is @c NULL, nothing is done. |
| 607 |
*/ |
| 608 |
|
| 609 |
void cpl_matrix_delete(cpl_matrix *matrix) |
| 610 |
{ |
| 611 |
|
| 612 |
if (matrix) { |
| 613 |
cpl_free(matrix->m); |
| 614 |
cpl_free(matrix); |
| 615 |
} |
| 616 |
|
| 617 |
} |
| 618 |
|
| 619 |
|
| 620 |
/** |
| 621 |
* @brief |
| 622 |
* Delete a matrix, but not its data buffer. |
| 623 |
* |
| 624 |
* @param matrix Pointer to a matrix to be deleted. |
| 625 |
* |
| 626 |
* @return Pointer to the internal data buffer. |
| 627 |
* |
| 628 |
* This function deallocates all the memory associated to a matrix, |
| 629 |
* with the exception of its data buffer. This type of destructor |
| 630 |
* should be used on matrices created with the @c cpl_matrix_wrap() |
| 631 |
* constructor, if the data buffer specified then was not allocated |
| 632 |
* using the functions of the @c cpl_memory module. In such a case, the |
| 633 |
* data buffer should be deallocated separately. See the documentation |
| 634 |
* of the function @c cpl_matrix_wrap(). If @em matrix is @c NULL, |
| 635 |
* nothing is done, and a @c NULL pointer is returned. |
| 636 |
*/ |
| 637 |
|
| 638 |
void *cpl_matrix_unwrap(cpl_matrix *matrix) |
| 639 |
{ |
| 640 |
|
| 641 |
void *data = NULL; |
| 642 |
|
| 643 |
if (matrix) { |
| 644 |
data = matrix->m; |
| 645 |
cpl_free(matrix); |
| 646 |
} |
| 647 |
|
| 648 |
return data; |
| 649 |
|
| 650 |
} |
| 651 |
|
| 652 |
|
| 653 |
/** |
| 654 |
* @brief |
| 655 |
* Get the number of rows of a matrix. |
| 656 |
* |
| 657 |
* @param matrix Pointer to the matrix to examine. |
| 658 |
* |
| 659 |
* @return Number of matrix rows, or zero in case of failure. |
| 660 |
* |
| 661 |
* @error |
| 662 |
* <table class="ec" align="center"> |
| 663 |
* <tr> |
| 664 |
* <td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 665 |
* <td class="ecr"> |
| 666 |
* The input <i>matrix</i> is a <tt>NULL</tt> pointer. |
| 667 |
* </td> |
| 668 |
* </tr> |
| 669 |
* </table> |
| 670 |
* @enderror |
| 671 |
* |
| 672 |
* Determine the number of rows in a matrix. |
| 673 |
*/ |
| 674 |
|
| 675 |
inline |
| 676 |
cpl_size cpl_matrix_get_nrow(const cpl_matrix *matrix) |
| 677 |
{ |
| 678 |
|
| 679 |
if (matrix == NULL) { |
| 680 |
/* inline precludes using cpl_error_set_() due to its use of __func__ */ |
| 681 |
(void) cpl_error_set("cpl_matrix_get_nrow", CPL_ERROR_NULL_INPUT); |
| 682 |
return 0; |
| 683 |
} |
| 684 |
|
| 685 |
return matrix->nr; |
| 686 |
|
| 687 |
} |
| 688 |
|
| 689 |
|
| 690 |
/** |
| 691 |
* @brief |
| 692 |
* Get the number of columns of a matrix. |
| 693 |
* |
| 694 |
* @param matrix Pointer to the matrix to examine. |
| 695 |
* |
| 696 |
* @return Number of matrix columns, or zero in case of failure. |
| 697 |
* |
| 698 |
* @error |
| 699 |
* <table class="ec" align="center"> |
| 700 |
* <tr> |
| 701 |
* <td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 702 |
* <td class="ecr"> |
| 703 |
* The input <i>matrix</i> is a <tt>NULL</tt> pointer. |
| 704 |
* </td> |
| 705 |
* </tr> |
| 706 |
* </table> |
| 707 |
* @enderror |
| 708 |
* |
| 709 |
* Determine the number of columns in a matrix. |
| 710 |
*/ |
| 711 |
|
| 712 |
inline |
| 713 |
cpl_size cpl_matrix_get_ncol(const cpl_matrix *matrix) |
| 714 |
{ |
| 715 |
|
| 716 |
if (matrix == NULL) { |
| 717 |
/* inline precludes using cpl_error_set_() due to its use of __func__ */ |
| 718 |
(void) cpl_error_set("cpl_matrix_get_ncol", CPL_ERROR_NULL_INPUT); |
| 719 |
return 0; |
| 720 |
} |
| 721 |
|
| 722 |
return matrix->nc; |
| 723 |
|
| 724 |
} |
| 725 |
|
| 726 |
|
| 727 |
/** |
| 728 |
* @brief |
| 729 |
* Get the pointer to a matrix data buffer, or @c NULL in case of error. |
| 730 |
* |
| 731 |
* @param matrix Input matrix. |
| 732 |
* |
| 733 |
* @return Pointer to the matrix data buffer. |
| 734 |
* |
| 735 |
* @error |
| 736 |
* <table class="ec" align="center"> |
| 737 |
* <tr> |
| 738 |
* <td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 739 |
* <td class="ecr"> |
| 740 |
* The input <i>matrix</i> is a <tt>NULL</tt> pointer. |
| 741 |
* </td> |
| 742 |
* </tr> |
| 743 |
* </table> |
| 744 |
* @enderror |
| 745 |
* |
| 746 |
* A @em cpl_matrix object includes an array of values of type @em double. |
| 747 |
* This function returns a pointer to this internal array, whose first |
| 748 |
* element corresponds to the @em cpl_matrix element 0,0. The internal |
| 749 |
* array contains in sequence all the @em cpl_matrix rows. For instance, |
| 750 |
* in the case of a 3x4 matrix, the array elements |
| 751 |
* @code |
| 752 |
* 0 1 2 3 4 5 6 7 8 9 10 11 |
| 753 |
* @endcode |
| 754 |
* would correspond to the following matrix elements: |
| 755 |
* @code |
| 756 |
* 0 1 2 3 |
| 757 |
* 4 5 6 7 |
| 758 |
* 8 9 10 11 |
| 759 |
* @endcode |
| 760 |
* |
| 761 |
* @note |
| 762 |
* Use at your own risk: direct manipulation of matrix data rules out |
| 763 |
* any check performed by the matrix object interface, and may introduce |
| 764 |
* inconsistencies between the information maintained internally, and |
| 765 |
* the actual matrix data and structure. |
| 766 |
*/ |
| 767 |
|
| 768 |
inline |
| 769 |
double *cpl_matrix_get_data(cpl_matrix *matrix) |
| 770 |
{ |
| 771 |
|
| 772 |
if (matrix == NULL) { |
| 773 |
/* inline precludes using cpl_error_set_() due to its use of __func__ */ |
| 774 |
(void) cpl_error_set("cpl_matrix_get_data", CPL_ERROR_NULL_INPUT); |
| 775 |
return NULL; |
| 776 |
} |
| 777 |
|
| 778 |
return matrix->m; |
| 779 |
|
| 780 |
} |
| 781 |
|
| 782 |
|
| 783 |
/** |
| 784 |
* @brief |
| 785 |
* Get the pointer to a matrix data buffer, or @c NULL in case of error. |
| 786 |
* |
| 787 |
* @param matrix Input matrix. |
| 788 |
* |
| 789 |
* @return Pointer to the matrix data buffer. |
| 790 |
* |
| 791 |
* @see cpl_matrix_get_data |
| 792 |
*/ |
| 793 |
|
| 794 |
inline |
| 795 |
const double *cpl_matrix_get_data_const(const cpl_matrix *matrix) |
| 796 |
{ |
| 797 |
if (matrix == NULL) { |
| 798 |
/* inline precludes using cpl_error_set_() due to its use of __func__ */ |
| 799 |
(void) cpl_error_set("cpl_matrix_get_data_const", CPL_ERROR_NULL_INPUT); |
| 800 |
return NULL; |
| 801 |
} |
| 802 |
|
| 803 |
return matrix->m; |
| 804 |
} |
| 805 |
|
| 806 |
|
| 807 |
/** |
| 808 |
* @brief |
| 809 |
* Get the value of a matrix element. |
| 810 |
* |
| 811 |
* @param matrix Pointer to a matrix. |
| 812 |
* @param row Matrix element row. |
| 813 |
* @param column Matrix element column. |
| 814 |
* |
| 815 |
* @return Value of the specified matrix element, or 0.0 in case of error. |
| 816 |
* |
| 817 |
* @error |
| 818 |
* <table class="ec" align="center"> |
| 819 |
* <tr> |
| 820 |
* <td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 821 |
* <td class="ecr"> |
| 822 |
* The input <i>matrix</i> is a <tt>NULL</tt> pointer. |
| 823 |
* </td> |
| 824 |
* </tr> |
| 825 |
* <tr> |
| 826 |
* <td class="ecl">CPL_ERROR_ACCESS_OUT_OF_RANGE</td> |
| 827 |
* <td class="ecr"> |
| 828 |
* The accessed element is beyond the <i>matrix</i> boundaries. |
| 829 |
* </td> |
| 830 |
* </tr> |
| 831 |
* </table> |
| 832 |
* @enderror |
| 833 |
* |
| 834 |
* Get the value of a matrix element. The matrix rows and columns are |
| 835 |
* counted from 0,0. |
| 836 |
*/ |
| 837 |
|
| 838 |
double cpl_matrix_get(const cpl_matrix *matrix, cpl_size row, cpl_size column) |
| 839 |
{ |
| 840 |
|
| 841 |
|
| 842 |
|
| 843 |
|
| 844 |
if (matrix == NULL) { |
| 845 |
(void)cpl_error_set_(CPL_ERROR_NULL_INPUT); |
| 846 |
return 0.0; |
| 847 |
} |
| 848 |
|
| 849 |
if (row < 0 || row >= matrix->nr || column < 0 || column >= matrix->nc) { |
| 850 |
(void)cpl_error_set_(CPL_ERROR_ACCESS_OUT_OF_RANGE); |
| 851 |
return 0.0; |
| 852 |
} |
| 853 |
|
| 854 |
return matrix->m[column + row * matrix->nc]; |
| 855 |
|
| 856 |
} |
| 857 |
|
| 858 |
|
| 859 |
/** |
| 860 |
* @brief |
| 861 |
* Write a value to a matrix element. |
| 862 |
* |
| 863 |
* @param matrix Input matrix. |
| 864 |
* @param row Matrix element row. |
| 865 |
* @param column Matrix element column. |
| 866 |
* @param value Value to write. |
| 867 |
* |
| 868 |
* @return @c CPL_ERROR_NONE on success. |
| 869 |
* |
| 870 |
* @error |
| 871 |
* <table class="ec" align="center"> |
| 872 |
* <tr> |
| 873 |
* <td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 874 |
* <td class="ecr"> |
| 875 |
* The input <i>matrix</i> is a <tt>NULL</tt> pointer. |
| 876 |
* </td> |
| 877 |
* </tr> |
| 878 |
* <tr> |
| 879 |
* <td class="ecl">CPL_ERROR_ACCESS_OUT_OF_RANGE</td> |
| 880 |
* <td class="ecr"> |
| 881 |
* The accessed element is beyond the <i>matrix</i> boundaries. |
| 882 |
* </td> |
| 883 |
* </tr> |
| 884 |
* </table> |
| 885 |
* @enderror |
| 886 |
* |
| 887 |
* Write a value to a matrix element. The matrix rows and columns are |
| 888 |
* counted from 0,0. |
| 889 |
*/ |
| 890 |
|
| 891 |
inline |
| 892 |
cpl_error_code cpl_matrix_set(cpl_matrix *matrix, |
| 893 |
cpl_size row, cpl_size column, double value) |
| 894 |
{ |
| 895 |
|
| 896 |
|
| 897 |
|
| 898 |
|
| 899 |
if (matrix == NULL) |
| 900 |
return cpl_error_set("cpl_matrix_set", CPL_ERROR_NULL_INPUT); |
| 901 |
|
| 902 |
if (row < 0 || row >= matrix->nr || column < 0 || column >= matrix->nc) |
| 903 |
return cpl_error_set("cpl_matrix_set", CPL_ERROR_ACCESS_OUT_OF_RANGE); |
| 904 |
|
| 905 |
matrix->m[column + row * matrix->nc] = value; |
| 906 |
|
| 907 |
return CPL_ERROR_NONE; |
| 908 |
|
| 909 |
} |
| 910 |
|
| 911 |
|
| 912 |
/** |
| 913 |
* @brief |
| 914 |
* Make a copy of a matrix. |
| 915 |
* |
| 916 |
* @param matrix Matrix to be duplicated. |
| 917 |
* |
| 918 |
* @return Pointer to the new matrix, or @c NULL in case of error. |
| 919 |
* |
| 920 |
* @error |
| 921 |
* <table class="ec" align="center"> |
| 922 |
* <tr> |
| 923 |
* <td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 924 |
* <td class="ecr"> |
| 925 |
* The input <i>matrix</i> is a <tt>NULL</tt> pointer. |
| 926 |
* </td> |
| 927 |
* </tr> |
| 928 |
* </table> |
| 929 |
* @enderror |
| 930 |
* |
| 931 |
* A copy of the input matrix is created. To destroy the duplicated matrix |
| 932 |
* the function @c cpl_matrix_delete() should be used. |
| 933 |
*/ |
| 934 |
|
| 935 |
cpl_matrix *cpl_matrix_duplicate(const cpl_matrix *matrix) |
| 936 |
{ |
| 937 |
|
| 938 |
|
| 939 |
cpl_matrix *new_matrix = NULL; |
| 940 |
|
| 941 |
if (matrix) { |
| 942 |
const cpl_size size = matrix->nr * matrix->nc; |
| 943 |
|
| 944 |
new_matrix = (cpl_matrix *)cpl_malloc(sizeof(cpl_matrix)); |
| 945 |
new_matrix->nr = matrix->nr; |
| 946 |
new_matrix->nc = matrix->nc; |
| 947 |
new_matrix->m = (double *)cpl_malloc((size_t)size * sizeof(double)); |
| 948 |
memcpy(new_matrix->m, matrix->m, (size_t)size * sizeof(double)); |
| 949 |
} |
| 950 |
else |
| 951 |
(void)cpl_error_set_(CPL_ERROR_NULL_INPUT); |
| 952 |
|
| 953 |
return new_matrix; |
| 954 |
|
| 955 |
} |
| 956 |
|
| 957 |
/** |
| 958 |
* @brief |
| 959 |
* Write the same value to all matrix elements. |
| 960 |
* |
| 961 |
* @param matrix Pointer to the matrix to access |
| 962 |
* @param value Value to write |
| 963 |
* |
| 964 |
* @return @c CPL_ERROR_NONE on success. |
| 965 |
* |
| 966 |
* @error |
| 967 |
* <table class="ec" align="center"> |
| 968 |
* <tr> |
| 969 |
* <td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 970 |
* <td class="ecr"> |
| 971 |
* The input <i>matrix</i> is a <tt>NULL</tt> pointer. |
| 972 |
* </td> |
| 973 |
* </tr> |
| 974 |
* </table> |
| 975 |
* @enderror |
| 976 |
* |
| 977 |
* Write the same value to all matrix elements. |
| 978 |
*/ |
| 979 |
|
| 980 |
cpl_error_code cpl_matrix_fill(cpl_matrix *matrix, double value) |
| 981 |
{ |
| 982 |
|
| 983 |
|
| 984 |
cpl_size size; |
| 985 |
double *m; |
| 986 |
|
| 987 |
|
| 988 |
if (matrix == NULL) |
| 989 |
return cpl_error_set_(CPL_ERROR_NULL_INPUT); |
| 990 |
|
| 991 |
size = matrix->nr * matrix->nc; |
| 992 |
m = matrix->m; |
| 993 |
|
| 994 |
while (size--) |
| 995 |
*m++ = value; |
| 996 |
|
| 997 |
return CPL_ERROR_NONE; |
| 998 |
|
| 999 |
} |
| 1000 |
|
| 1001 |
|
| 1002 |
/** |
| 1003 |
* @brief |
| 1004 |
* Write the same value to a matrix row. |
| 1005 |
* |
| 1006 |
* @param matrix Matrix to access |
| 1007 |
* @param value Value to write |
| 1008 |
* @param row Sequence number of row to overwrite. |
| 1009 |
* |
| 1010 |
* @return @c CPL_ERROR_NONE on success. |
| 1011 |
* |
| 1012 |
* @error |
| 1013 |
* <table class="ec" align="center"> |
| 1014 |
* <tr> |
| 1015 |
* <td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 1016 |
* <td class="ecr"> |
| 1017 |
* The input <i>matrix</i> is a <tt>NULL</tt> pointer. |
| 1018 |
* </td> |
| 1019 |
* </tr> |
| 1020 |
* <tr> |
| 1021 |
* <td class="ecl">CPL_ERROR_ACCESS_OUT_OF_RANGE</td> |
| 1022 |
* <td class="ecr"> |
| 1023 |
* The specified <i>row</i> is outside the <i>matrix</i> boundaries. |
| 1024 |
* </td> |
| 1025 |
* </tr> |
| 1026 |
* </table> |
| 1027 |
* @enderror |
| 1028 |
* |
| 1029 |
* Write the same value to a matrix row. Rows are counted starting from 0. |
| 1030 |
*/ |
| 1031 |
|
| 1032 |
cpl_error_code cpl_matrix_fill_row(cpl_matrix *matrix, double value, |
| 1033 |
cpl_size row) |
| 1034 |
{ |
| 1035 |
|
| 1036 |
|
| 1037 |
double *m; |
| 1038 |
cpl_size count; |
| 1039 |
|
| 1040 |
|
| 1041 |
if (matrix == NULL) |
| 1042 |
return cpl_error_set_(CPL_ERROR_NULL_INPUT); |
| 1043 |
|
| 1044 |
if (row < 0 || row >= matrix->nr) |
| 1045 |
return cpl_error_set_(CPL_ERROR_ACCESS_OUT_OF_RANGE); |
| 1046 |
|
| 1047 |
m = matrix->m + row * matrix->nc; |
| 1048 |
count = matrix->nc; |
| 1049 |
|
| 1050 |
while (count--) |
| 1051 |
*m++ = value; |
| 1052 |
|
| 1053 |
return CPL_ERROR_NONE; |
| 1054 |
|
| 1055 |
} |
| 1056 |
|
| 1057 |
|
| 1058 |
/** |
| 1059 |
* @brief |
| 1060 |
* Write the same value to a matrix column. |
| 1061 |
* |
| 1062 |
* @param matrix Pointer to the matrix to access |
| 1063 |
* @param value Value to write |
| 1064 |
* @param column Sequence number of column to overwrite |
| 1065 |
* |
| 1066 |
* @return @c CPL_ERROR_NONE on success. |
| 1067 |
* |
| 1068 |
* @error |
| 1069 |
* <table class="ec" align="center"> |
| 1070 |
* <tr> |
| 1071 |
* <td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 1072 |
* <td class="ecr"> |
| 1073 |
* The input <i>matrix</i> is a <tt>NULL</tt> pointer. |
| 1074 |
* </td> |
| 1075 |
* </tr> |
| 1076 |
* <tr> |
| 1077 |
* <td class="ecl">CPL_ERROR_ACCESS_OUT_OF_RANGE</td> |
| 1078 |
* <td class="ecr"> |
| 1079 |
* The specified <i>column</i> is outside the <i>matrix</i> boundaries. |
| 1080 |
* </td> |
| 1081 |
* </tr> |
| 1082 |
* </table> |
| 1083 |
* @enderror |
| 1084 |
* |
| 1085 |
* Write the same value to a matrix column. Columns are counted starting |
| 1086 |
* from 0. |
| 1087 |
*/ |
| 1088 |
|
| 1089 |
cpl_error_code cpl_matrix_fill_column(cpl_matrix *matrix, |
| 1090 |
double value, cpl_size column) |
| 1091 |
{ |
| 1092 |
|
| 1093 |
|
| 1094 |
cpl_size count; |
| 1095 |
double *m; |
| 1096 |
|
| 1097 |
|
| 1098 |
if (matrix == NULL) |
| 1099 |
return cpl_error_set_(CPL_ERROR_NULL_INPUT); |
| 1100 |
|
| 1101 |
if (column < 0 || column >= matrix->nc) |
| 1102 |
return cpl_error_set_(CPL_ERROR_ACCESS_OUT_OF_RANGE); |
| 1103 |
|
| 1104 |
count = matrix->nr; |
| 1105 |
m = matrix->m + column; |
| 1106 |
|
| 1107 |
while (count--) { |
| 1108 |
*m = value; |
| 1109 |
m += matrix->nc; |
| 1110 |
} |
| 1111 |
|
| 1112 |
return CPL_ERROR_NONE; |
| 1113 |
|
| 1114 |
} |
| 1115 |
|
| 1116 |
|
| 1117 |
|
| 1118 |
/** |
| 1119 |
* @brief |
| 1120 |
* Write the values of a matrix into another matrix. |
| 1121 |
* |
| 1122 |
* @param matrix Pointer to matrix to be modified. |
| 1123 |
* @param submatrix Pointer to matrix to get the values from. |
| 1124 |
* @param row Position of row 0 of @em submatrix in @em matrix. |
| 1125 |
* @param col Position of column 0 of @em submatrix in @em matrix. |
| 1126 |
* |
| 1127 |
* @return @c CPL_ERROR_NONE on success. |
| 1128 |
* |
| 1129 |
* @error |
| 1130 |
* <table class="ec" align="center"> |
| 1131 |
* <tr> |
| 1132 |
* <td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 1133 |
* <td class="ecr"> |
| 1134 |
* The input <i>matrix</i> or <i>submatrix</i> are <tt>NULL</tt> |
| 1135 |
* pointers. |
| 1136 |
* </td> |
| 1137 |
* </tr> |
| 1138 |
* <tr> |
| 1139 |
* <td class="ecl">CPL_ERROR_ACCESS_OUT_OF_RANGE</td> |
| 1140 |
* <td class="ecr"> |
| 1141 |
* No overlap exists between the two matrices. |
| 1142 |
* </td> |
| 1143 |
* </tr> |
| 1144 |
* </table> |
| 1145 |
* @enderror |
| 1146 |
* |
| 1147 |
* The values of @em submatrix are written to @em matrix starting at the |
| 1148 |
* indicated row and column. There are no restrictions on the sizes of |
| 1149 |
* @em submatrix: just the parts of @em submatrix overlapping @em matrix |
| 1150 |
* are copied. There are no restrictions on @em row and @em col either, |
| 1151 |
* that can also be negative. If the two matrices do not overlap, nothing |
| 1152 |
* is done, but an error condition is set. |
| 1153 |
*/ |
| 1154 |
|
| 1155 |
cpl_error_code cpl_matrix_copy(cpl_matrix *matrix, const cpl_matrix *submatrix, |
| 1156 |
cpl_size row, cpl_size col) |
| 1157 |
{ |
| 1158 |
|
| 1159 |
|
| 1160 |
cpl_size endrow; |
| 1161 |
cpl_size endcol; |
| 1162 |
cpl_size subrow; |
| 1163 |
cpl_size subcol; |
| 1164 |
cpl_size r, c, sr, sc; |
| 1165 |
double *m; |
| 1166 |
double *sm; |
| 1167 |
|
| 1168 |
|
| 1169 |
if (matrix == NULL || submatrix == NULL) |
| 1170 |
return cpl_error_set_(CPL_ERROR_NULL_INPUT); |
| 1171 |
|
| 1172 |
|
| 1173 |
endrow = row + submatrix->nr; |
| 1174 |
endcol = col + submatrix->nc; |
| 1175 |
|
| 1176 |
|
| 1177 |
/* |
| 1178 |
* Check whether matrices overlap. |
| 1179 |
*/ |
| 1180 |
|
| 1181 |
if (row >= matrix->nr || endrow < 1 || col >= matrix->nc || endcol < 1) |
| 1182 |
return cpl_error_set_(CPL_ERROR_ACCESS_OUT_OF_RANGE); |
| 1183 |
|
| 1184 |
|
| 1185 |
/* |
| 1186 |
* Define the overlap region on both matrices reference system: |
| 1187 |
*/ |
| 1188 |
|
| 1189 |
if (row < 0) { |
| 1190 |
subrow = -row; |
| 1191 |
row = 0; |
| 1192 |
} |
| 1193 |
else |
| 1194 |
subrow = 0; |
| 1195 |
|
| 1196 |
if (col < 0) { |
| 1197 |
subcol = -col; |
| 1198 |
col = 0; |
| 1199 |
} |
| 1200 |
else |
| 1201 |
subcol = 0; |
| 1202 |
|
| 1203 |
if (endrow > matrix->nr) |
| 1204 |
endrow = matrix->nr; |
| 1205 |
|
| 1206 |
if (endcol > matrix->nc) |
| 1207 |
endcol = matrix->nc; |
| 1208 |
|
| 1209 |
for (r = row, sr = subrow; r < endrow; r++, sr++) { |
| 1210 |
m = matrix->m + r * matrix->nc + col; |
| 1211 |
sm = submatrix->m + sr * submatrix->nc + subcol; |
| 1212 |
for (c = col, sc = subcol; c < endcol; c++, sc++) |
| 1213 |
*m++ = *sm++; |
| 1214 |
} |
| 1215 |
|
| 1216 |
return CPL_ERROR_NONE; |
| 1217 |
|
| 1218 |
} |
| 1219 |
|
| 1220 |
|
| 1221 |
/** |
| 1222 |
* @brief |
| 1223 |
* Write the same value into a submatrix of a matrix. |
| 1224 |
* |
| 1225 |
* @param matrix Pointer to matrix to be modified. |
| 1226 |
* @param value Value to write. |
| 1227 |
* @param row Start row of matrix submatrix. |
| 1228 |
* @param col Start column of matrix submatrix. |
| 1229 |
* @param nrow Number of rows of matrix submatrix. |
| 1230 |
* @param ncol Number of columns of matrix submatrix. |
| 1231 |
* |
| 1232 |
* @return @c CPL_ERROR_NONE on success. |
| 1233 |
* |
| 1234 |
* @error |
| 1235 |
* <table class="ec" align="center"> |
| 1236 |
* <tr> |
| 1237 |
* <td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 1238 |
* <td class="ecr"> |
| 1239 |
* The input <i>matrix</i> is a <tt>NULL</tt> pointer. |
| 1240 |
* </td> |
| 1241 |
* </tr> |
| 1242 |
* <tr> |
| 1243 |
* <td class="ecl">CPL_ERROR_ACCESS_OUT_OF_RANGE</td> |
| 1244 |
* <td class="ecr"> |
| 1245 |
* The specified start position is outside the <i>matrix</i> |
| 1246 |
* boundaries. |
| 1247 |
* </td> |
| 1248 |
* </tr> |
| 1249 |
* <tr> |
| 1250 |
* <td class="ecl">CPL_ERROR_ILLEGAL_INPUT</td> |
| 1251 |
* <td class="ecr"> |
| 1252 |
* <i>nrow</i> or <i>ncol</i> are not positive. |
| 1253 |
* </td> |
| 1254 |
* </tr> |
| 1255 |
* </table> |
| 1256 |
* @enderror |
| 1257 |
* |
| 1258 |
* The specified value is written to @em matrix starting at the indicated |
| 1259 |
* row and column; @em nrow and @em ncol can exceed the input matrix |
| 1260 |
* boundaries, just the range overlapping the @em matrix is used in that |
| 1261 |
* case. |
| 1262 |
*/ |
| 1263 |
|
| 1264 |
cpl_error_code cpl_matrix_fill_window(cpl_matrix *matrix, double value, |
| 1265 |
cpl_size row, cpl_size col, |
| 1266 |
cpl_size nrow, cpl_size ncol) |
| 1267 |
{ |
| 1268 |
|
| 1269 |
|
| 1270 |
cpl_size endrow; |
| 1271 |
cpl_size endcol; |
| 1272 |
cpl_size r, c; |
| 1273 |
double *m; |
| 1274 |
|
| 1275 |
|
| 1276 |
if (matrix == NULL) |
| 1277 |
return cpl_error_set_(CPL_ERROR_NULL_INPUT); |
| 1278 |
|
| 1279 |
if (row < 0 || row >= matrix->nr || col < 0 || col >= matrix->nc) |
| 1280 |
return cpl_error_set_(CPL_ERROR_ACCESS_OUT_OF_RANGE); |
| 1281 |
|
| 1282 |
if (nrow < 1 || ncol < 1) |
| 1283 |
return cpl_error_set_(CPL_ERROR_ILLEGAL_INPUT); |
| 1284 |
|
| 1285 |
endrow = row + nrow; |
| 1286 |
|
| 1287 |
if (endrow > matrix->nr) |
| 1288 |
endrow = matrix->nr; |
| 1289 |
|
| 1290 |
endcol = col + ncol; |
| 1291 |
|
| 1292 |
if (endcol > matrix->nc) |
| 1293 |
endcol = matrix->nc; |
| 1294 |
|
| 1295 |
for (r = row; r < endrow; r++) { |
| 1296 |
m = matrix->m + r * matrix->nc + col; |
| 1297 |
for (c = col; c < endcol; c++) |
| 1298 |
*m++ = value; |
| 1299 |
} |
| 1300 |
|
| 1301 |
return CPL_ERROR_NONE; |
| 1302 |
|
| 1303 |
} |
| 1304 |
|
| 1305 |
|
| 1306 |
/** |
| 1307 |
* @brief |
| 1308 |
* Swap two matrix columns. |
| 1309 |
* |
| 1310 |
* @param matrix Pointer to matrix to be modified. |
| 1311 |
* @param column1 One matrix column. |
| 1312 |
* @param column2 Another matrix column. |
| 1313 |
* |
| 1314 |
* @return @c CPL_ERROR_NONE on success. |
| 1315 |
* |
| 1316 |
* @error |
| 1317 |
* <table class="ec" align="center"> |
| 1318 |
* <tr> |
| 1319 |
* <td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 1320 |
* <td class="ecr"> |
| 1321 |
* The input <i>matrix</i> is a <tt>NULL</tt> pointer. |
| 1322 |
* </td> |
| 1323 |
* </tr> |
| 1324 |
* <tr> |
| 1325 |
* <td class="ecl">CPL_ERROR_ACCESS_OUT_OF_RANGE</td> |
| 1326 |
* <td class="ecr"> |
| 1327 |
* Any of the specified columns is outside the <i>matrix</i> |
| 1328 |
* boundaries. |
| 1329 |
* </td> |
| 1330 |
* </tr> |
| 1331 |
* </table> |
| 1332 |
* @enderror |
| 1333 |
* |
| 1334 |
* The values of two given matrix columns are exchanged. Columns are |
| 1335 |
* counted starting from 0. If the same column number is given twice, |
| 1336 |
* nothing is done and no error is set. |
| 1337 |
*/ |
| 1338 |
|
| 1339 |
cpl_error_code cpl_matrix_swap_columns(cpl_matrix *matrix, |
| 1340 |
cpl_size column1, cpl_size column2) |
| 1341 |
{ |
| 1342 |
|
| 1343 |
|
| 1344 |
double swap; |
| 1345 |
cpl_size nrow; |
| 1346 |
|
| 1347 |
|
| 1348 |
if (matrix == NULL) |
| 1349 |
return cpl_error_set_(CPL_ERROR_NULL_INPUT); |
| 1350 |
|
| 1351 |
if (column1 < 0 || column1 >= matrix->nc || |
| 1352 |
column2 < 0 || column2 >= matrix->nc) |
| 1353 |
return cpl_error_set_(CPL_ERROR_ACCESS_OUT_OF_RANGE); |
| 1354 |
|
| 1355 |
|
| 1356 |
if (column1 == column2) |
| 1357 |
return CPL_ERROR_NONE; |
| 1358 |
|
| 1359 |
nrow = matrix->nr; |
| 1360 |
|
| 1361 |
while (nrow--) { |
| 1362 |
swap = matrix->m[column1]; |
| 1363 |
matrix->m[column1] = matrix->m[column2]; |
| 1364 |
matrix->m[column2] = swap; |
| 1365 |
column1 += matrix->nc; |
| 1366 |
column2 += matrix->nc; |
| 1367 |
} |
| 1368 |
|
| 1369 |
return CPL_ERROR_NONE; |
| 1370 |
|
| 1371 |
} |
| 1372 |
|
| 1373 |
|
| 1374 |
/** |
| 1375 |
* @brief |
| 1376 |
* Swap a matrix column with a matrix row. |
| 1377 |
* |
| 1378 |
* @param matrix Pointer to matrix to be modified. |
| 1379 |
* @param row Matrix row. |
| 1380 |
* |
| 1381 |
* @return @c CPL_ERROR_NONE on success. |
| 1382 |
* |
| 1383 |
* @error |
| 1384 |
* <table class="ec" align="center"> |
| 1385 |
* <tr> |
| 1386 |
* <td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 1387 |
* <td class="ecr"> |
| 1388 |
* The input <i>matrix</i> is a <tt>NULL</tt> pointer. |
| 1389 |
* </td> |
| 1390 |
* </tr> |
| 1391 |
* <tr> |
| 1392 |
* <td class="ecl">CPL_ERROR_ACCESS_OUT_OF_RANGE</td> |
| 1393 |
* <td class="ecr"> |
| 1394 |
* The specified <i>row</i> is outside the <i>matrix</i> boundaries. |
| 1395 |
* </td> |
| 1396 |
* </tr> |
| 1397 |
* <tr> |
| 1398 |
* <td class="ecl">CPL_ERROR_ILLEGAL_INPUT</td> |
| 1399 |
* <td class="ecr"> |
| 1400 |
* The input <i>matrix</i> is not square. |
| 1401 |
* </td> |
| 1402 |
* </tr> |
| 1403 |
* </table> |
| 1404 |
* @enderror |
| 1405 |
* |
| 1406 |
* The values of the indicated row are exchanged with the column having |
| 1407 |
* the same sequence number. Rows and columns are counted starting from 0. |
| 1408 |
*/ |
| 1409 |
|
| 1410 |
cpl_error_code cpl_matrix_swap_rowcolumn(cpl_matrix *matrix, cpl_size row) |
| 1411 |
{ |
| 1412 |
|
| 1413 |
|
| 1414 |
double swap; |
| 1415 |
cpl_size i; |
| 1416 |
cpl_size posr, posc; |
| 1417 |
|
| 1418 |
|
| 1419 |
if (matrix == NULL) |
| 1420 |
return cpl_error_set_(CPL_ERROR_NULL_INPUT); |
| 1421 |
|
| 1422 |
if (matrix->nr != matrix->nc) |
| 1423 |
return cpl_error_set_(CPL_ERROR_ILLEGAL_INPUT); |
| 1424 |
|
| 1425 |
if (row < 0 || row >= matrix->nr) |
| 1426 |
return cpl_error_set_(CPL_ERROR_ACCESS_OUT_OF_RANGE); |
| 1427 |
|
| 1428 |
|
| 1429 |
posr = row; |
| 1430 |
posc = row * matrix->nc; |
| 1431 |
|
| 1432 |
for (i = 0; i < matrix->nr; i++) { |
| 1433 |
swap = matrix->m[posr]; |
| 1434 |
matrix->m[posr] = matrix->m[posc]; |
| 1435 |
matrix->m[posc] = swap; |
| 1436 |
posr += matrix->nc; |
| 1437 |
posc++; |
| 1438 |
} |
| 1439 |
|
| 1440 |
return CPL_ERROR_NONE; |
| 1441 |
|
| 1442 |
} |
| 1443 |
|
| 1444 |
|
| 1445 |
/** |
| 1446 |
* @brief |
| 1447 |
* Reverse order of rows in matrix. |
| 1448 |
* |
| 1449 |
* @param matrix Pointer to matrix to be reversed. |
| 1450 |
* |
| 1451 |
* @return @c CPL_ERROR_NONE on success. |
| 1452 |
* |
| 1453 |
* @error |
| 1454 |
* <table class="ec" align="center"> |
| 1455 |
* <tr> |
| 1456 |
* <td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 1457 |
* <td class="ecr"> |
| 1458 |
* The input <i>matrix</i> is a <tt>NULL</tt> pointer. |
| 1459 |
* </td> |
| 1460 |
* </tr> |
| 1461 |
* </table> |
| 1462 |
* @enderror |
| 1463 |
* |
| 1464 |
* The order of the rows in the matrix is reversed in place. |
| 1465 |
*/ |
| 1466 |
|
| 1467 |
cpl_error_code cpl_matrix_flip_rows(cpl_matrix *matrix) |
| 1468 |
{ |
| 1469 |
|
| 1470 |
|
| 1471 |
cpl_size i, j; |
| 1472 |
|
| 1473 |
|
| 1474 |
if (matrix == NULL) |
| 1475 |
return cpl_error_set_(CPL_ERROR_NULL_INPUT); |
| 1476 |
|
| 1477 |
|
| 1478 |
for (i = 0, j = matrix->nr - 1; i < j; i++, j--) |
| 1479 |
swap_rows(matrix, i, j); |
| 1480 |
|
| 1481 |
return CPL_ERROR_NONE; |
| 1482 |
|
| 1483 |
} |
| 1484 |
|
| 1485 |
|
| 1486 |
/** |
| 1487 |
* @brief |
| 1488 |
* Reverse order of columns in matrix. |
| 1489 |
* |
| 1490 |
* @param matrix Pointer to matrix to be reversed. |
| 1491 |
* |
| 1492 |
* @return @c CPL_ERROR_NONE on success. |
| 1493 |
* |
| 1494 |
* @error |
| 1495 |
* <table class="ec" align="center"> |
| 1496 |
* <tr> |
| 1497 |
* <td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 1498 |
* <td class="ecr"> |
| 1499 |
* The input <i>matrix</i> is a <tt>NULL</tt> pointer. |
| 1500 |
* </td> |
| 1501 |
* </tr> |
| 1502 |
* </table> |
| 1503 |
* @enderror |
| 1504 |
* |
| 1505 |
* The order of the columns in the matrix is reversed in place. |
| 1506 |
*/ |
| 1507 |
|
| 1508 |
cpl_error_code cpl_matrix_flip_columns(cpl_matrix *matrix) |
| 1509 |
{ |
| 1510 |
|
| 1511 |
|
| 1512 |
cpl_size i, j; |
| 1513 |
|
| 1514 |
|
| 1515 |
if (matrix == NULL) |
| 1516 |
return cpl_error_set_(CPL_ERROR_NULL_INPUT); |
| 1517 |
|
| 1518 |
|
| 1519 |
for (i = 0, j = matrix->nc - 1; i < j; i++, j--) |
| 1520 |
cpl_matrix_swap_columns(matrix, i, j); |
| 1521 |
|
| 1522 |
return CPL_ERROR_NONE; |
| 1523 |
|
| 1524 |
} |
| 1525 |
|
| 1526 |
|
| 1527 |
/** |
| 1528 |
* @brief |
| 1529 |
* Create transposed matrix. |
| 1530 |
* |
| 1531 |
* @param matrix Pointer to matrix to be transposed. |
| 1532 |
* |
| 1533 |
* @return Pointer to transposed matrix. If a @c NULL pointer is passed, |
| 1534 |
* a @c NULL pointer is returned. |
| 1535 |
* |
| 1536 |
* @error |
| 1537 |
* <table class="ec" align="center"> |
| 1538 |
* <tr> |
| 1539 |
* <td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 1540 |
* <td class="ecr"> |
| 1541 |
* The input <i>matrix</i> is a <tt>NULL</tt> pointer. |
| 1542 |
* </td> |
| 1543 |
* </tr> |
| 1544 |
* </table> |
| 1545 |
* @enderror |
| 1546 |
* |
| 1547 |
* The transposed of the input matrix is created. To destroy the new matrix |
| 1548 |
* the function @c cpl_matrix_delete() should be used. |
| 1549 |
*/ |
| 1550 |
|
| 1551 |
cpl_matrix *cpl_matrix_transpose_create(const cpl_matrix *matrix) |
| 1552 |
{ |
| 1553 |
|
| 1554 |
|
| 1555 |
cpl_matrix *transposed = NULL; |
| 1556 |
cpl_size i, j; |
| 1557 |
double *m; |
| 1558 |
double *tm; |
| 1559 |
|
| 1560 |
|
| 1561 |
if (matrix == NULL) { |
| 1562 |
cpl_error_set_(CPL_ERROR_NULL_INPUT); |
| 1563 |
return NULL; |
| 1564 |
} |
| 1565 |
|
| 1566 |
|
| 1567 |
tm = (double *)cpl_malloc((size_t)matrix->nc * (size_t)matrix->nr |
| 1568 |
* sizeof(*tm)); |
| 1569 |
transposed = cpl_matrix_wrap(matrix->nc, matrix->nr, tm); |
| 1570 |
m = matrix->m; |
| 1571 |
|
| 1572 |
for (i = 0; i < matrix->nr; i++) |
| 1573 |
for (j = 0, tm = transposed->m + i; |
| 1574 |
j < matrix->nc; j++, tm += matrix->nr) |
| 1575 |
*tm = *m++; |
| 1576 |
|
| 1577 |
return transposed; |
| 1578 |
|
| 1579 |
} |
| 1580 |
|
| 1581 |
|
| 1582 |
/** |
| 1583 |
* @brief |
| 1584 |
* Reframe a matrix. |
| 1585 |
* |
| 1586 |
* @param matrix Pointer to matrix to be modified. |
| 1587 |
* @param top Extra rows on top. |
| 1588 |
* @param bottom Extra rows on bottom. |
| 1589 |
* @param left Extra columns on left. |
| 1590 |
* @param right Extra columns on right. |
| 1591 |
* |
| 1592 |
* @return @c CPL_ERROR_NONE on success. |
| 1593 |
* |
| 1594 |
* @error |
| 1595 |
* <table class="ec" align="center"> |
| 1596 |
* <tr> |
| 1597 |
* <td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 1598 |
* <td class="ecr"> |
| 1599 |
* The input <i>matrix</i> is a <tt>NULL</tt> pointer. |
| 1600 |
* </td> |
| 1601 |
* </tr> |
| 1602 |
* <tr> |
| 1603 |
* <td class="ecl">CPL_ERROR_ILLEGAL_OUTPUT</td> |
| 1604 |
* <td class="ecr"> |
| 1605 |
* Attempt to shrink <i>matrix</i> to zero size (or less). |
| 1606 |
* </td> |
| 1607 |
* </tr> |
| 1608 |
* </table> |
| 1609 |
* @enderror |
| 1610 |
* |
| 1611 |
* The input matrix is reframed according to specifications. Extra rows |
| 1612 |
* and column on the sides might also be negative, as long as they are |
| 1613 |
* compatible with the matrix sizes: the input matrix would be reduced |
| 1614 |
* in size accordingly, but an attempt to remove all matrix columns |
| 1615 |
* and/or rows is flagged as an error because zero length matrices are |
| 1616 |
* illegal. The old matrix elements contained in the new shape are left |
| 1617 |
* unchanged, and new matrix elements added by the reshaping are initialised |
| 1618 |
* to zero. No reshaping (i.e., all the extra rows set to zero) would |
| 1619 |
* not be flagged as an error. |
| 1620 |
* |
| 1621 |
* @note |
| 1622 |
* The pointer to the matrix data buffer may change, therefore |
| 1623 |
* pointers previously retrieved by calling @c cpl_matrix_get_data() |
| 1624 |
* should be discarded. |
| 1625 |
*/ |
| 1626 |
|
| 1627 |
cpl_error_code cpl_matrix_resize(cpl_matrix *matrix, |
| 1628 |
cpl_size top, cpl_size bottom, |
| 1629 |
cpl_size left, cpl_size right) |
| 1630 |
{ |
| 1631 |
|
| 1632 |
|
| 1633 |
cpl_matrix *resized; |
| 1634 |
cpl_size nr, nc; |
| 1635 |
|
| 1636 |
|
| 1637 |
if (matrix == NULL) |
| 1638 |
return cpl_error_set_(CPL_ERROR_NULL_INPUT); |
| 1639 |
|
| 1640 |
|
| 1641 |
if (top == 0 && bottom == 0 && left == 0 && right == 0) |
| 1642 |
return CPL_ERROR_NONE; |
| 1643 |
|
| 1644 |
nr = matrix->nr + top + bottom; |
| 1645 |
nc = matrix->nc + left + right; |
| 1646 |
|
| 1647 |
if (nr < 1 || nc < 1) |
| 1648 |
return cpl_error_set_(CPL_ERROR_ILLEGAL_OUTPUT); |
| 1649 |
|
| 1650 |
resized = cpl_matrix_new(nr, nc); |
| 1651 |
|
| 1652 |
cpl_matrix_copy(resized, matrix, top, left); |
| 1653 |
|
| 1654 |
cpl_free(matrix->m); |
| 1655 |
matrix->m = cpl_matrix_unwrap(resized); |
| 1656 |
matrix->nr = nr; |
| 1657 |
matrix->nc = nc; |
| 1658 |
|
| 1659 |
return CPL_ERROR_NONE; |
| 1660 |
|
| 1661 |
} |
| 1662 |
|
| 1663 |
|
| 1664 |
/** |
| 1665 |
* @brief |
| 1666 |
* Resize a matrix. |
| 1667 |
* |
| 1668 |
* @param matrix Pointer to matrix to be resized. |
| 1669 |
* @param rows New number of rows. |
| 1670 |
* @param columns New number of columns. |
| 1671 |
* |
| 1672 |
* @return @c CPL_ERROR_NONE on success. |
| 1673 |
* |
| 1674 |
* @error |
| 1675 |
* <table class="ec" align="center"> |
| 1676 |
* <tr> |
| 1677 |
* <td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 1678 |
* <td class="ecr"> |
| 1679 |
* The input <i>matrix</i> is a <tt>NULL</tt> pointer. |
| 1680 |
* </td> |
| 1681 |
* </tr> |
| 1682 |
* <tr> |
| 1683 |
* <td class="ecl">CPL_ERROR_ILLEGAL_OUTPUT</td> |
| 1684 |
* <td class="ecr"> |
| 1685 |
* Attempt to shrink <i>matrix</i> to zero size (or less). |
| 1686 |
* </td> |
| 1687 |
* </tr> |
| 1688 |
* </table> |
| 1689 |
* @enderror |
| 1690 |
* |
| 1691 |
* The input matrix is resized according to specifications. The old |
| 1692 |
* matrix elements contained in the resized matrix are left unchanged, |
| 1693 |
* and new matrix elements added by an increase of the matrix number of |
| 1694 |
* rows and/or columns are initialised to zero. |
| 1695 |
* |
| 1696 |
* @note |
| 1697 |
* The pointer to the matrix data buffer may change, therefore |
| 1698 |
* pointers previously retrieved by calling @c cpl_matrix_get_data() |
| 1699 |
* should be discarded. |
| 1700 |
*/ |
| 1701 |
|
| 1702 |
cpl_error_code cpl_matrix_set_size(cpl_matrix *matrix, cpl_size rows, |
| 1703 |
cpl_size columns) |
| 1704 |
{ |
| 1705 |
|
| 1706 |
return cpl_matrix_resize(matrix, 0, rows - matrix->nr, |
| 1707 |
0, columns - matrix->nc) |
| 1708 |
? cpl_error_set_where_() : CPL_ERROR_NONE; |
| 1709 |
|
| 1710 |
} |
| 1711 |
|
| 1712 |
|
| 1713 |
/** |
| 1714 |
* @brief |
| 1715 |
* Rows-by-columns product of two matrices. |
| 1716 |
* |
| 1717 |
* @param matrix1 Pointer to left side matrix. |
| 1718 |
* @param matrix2 Pointer to right side matrix. |
| 1719 |
* |
| 1720 |
* @return Pointer to product matrix, or @c NULL in case of error. |
| 1721 |
* |
| 1722 |
* @error |
| 1723 |
* <table class="ec" align="center"> |
| 1724 |
* <tr> |
| 1725 |
* <td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 1726 |
* <td class="ecr"> |
| 1727 |
* An input matrix is a <tt>NULL</tt> pointer. |
| 1728 |
* </td> |
| 1729 |
* </tr> |
| 1730 |
* <tr> |
| 1731 |
* <td class="ecl">CPL_ERROR_INCOMPATIBLE_INPUT</td> |
| 1732 |
* <td class="ecr"> |
| 1733 |
* The number of columns of the first matrix is not equal to |
| 1734 |
* the number of rows of the second matrix. |
| 1735 |
* </td> |
| 1736 |
* </tr> |
| 1737 |
* </table> |
| 1738 |
* @enderror |
| 1739 |
* |
| 1740 |
* Rows-by-columns product of two matrices. The number of columns of the |
| 1741 |
* first matrix must be equal to the number of rows of the second matrix. |
| 1742 |
* To destroy the new matrix the function @c cpl_matrix_delete() should |
| 1743 |
* be used. |
| 1744 |
*/ |
| 1745 |
|
| 1746 |
cpl_matrix *cpl_matrix_product_create(const cpl_matrix *matrix1, |
| 1747 |
const cpl_matrix *matrix2) |
| 1748 |
{ |
| 1749 |
|
| 1750 |
cpl_matrix *self; |
| 1751 |
|
| 1752 |
cpl_ensure(matrix1 != NULL, CPL_ERROR_NULL_INPUT, NULL); |
| 1753 |
cpl_ensure(matrix2 != NULL, CPL_ERROR_NULL_INPUT, NULL); |
| 1754 |
cpl_ensure(matrix1->nc == matrix2->nr, CPL_ERROR_INCOMPATIBLE_INPUT, NULL); |
| 1755 |
|
| 1756 |
/* Create data-buffer without overhead of initializing */ |
| 1757 |
self = cpl_matrix_wrap(matrix1->nr, matrix2->nc, |
| 1758 |
cpl_calloc((size_t)matrix1->nr * (size_t)matrix2->nc |
| 1759 |
, sizeof(*self))); |
| 1760 |
|
| 1761 |
(void) cpl_matrix_product(self, matrix1, matrix2); |
| 1762 |
|
| 1763 |
return self; |
| 1764 |
|
| 1765 |
} |
| 1766 |
|
| 1767 |
|
| 1768 |
/* |
| 1769 |
* @brief Replace a matrix by its LU-decomposition |
| 1770 |
* @param self n X n non-singular matrix to decompose |
| 1771 |
* @param perm n-integer array to be filled with the row permutations |
| 1772 |
* @param psig On success set to 1/-1 for an even/odd number of permutations |
| 1773 |
* @return CPL_ERROR_NONE on success, or the relevant CPL error code |
| 1774 |
* @note If the matrix is singular the elements of self become undefined |
| 1775 |
* @see Golub & Van Loan, Matrix Computations, Algorithms 3.2.1 (Outer Product |
| 1776 |
* Gaussian Elimination) and 3.4.1 (Gauss Elimination with Partial Pivoting). |
| 1777 |
* |
| 1778 |
* @error |
| 1779 |
* <table class="ec" align="center"> |
| 1780 |
* <tr> |
| 1781 |
* <td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 1782 |
* <td class="ecr"> |
| 1783 |
* An input pointer is <tt>NULL</tt>. |
| 1784 |
* </td> |
| 1785 |
* </tr> |
| 1786 |
* <tr> |
| 1787 |
* <td class="ecl">CPL_ERROR_ILLEGAL_INPUT</td> |
| 1788 |
* <td class="ecr"> |
| 1789 |
* <i>self</i> is not an n by n matrix. |
| 1790 |
* </td> |
| 1791 |
* </tr> |
| 1792 |
* <tr> |
| 1793 |
* <td class="ecl">CPL_ERROR_INCOMPATIBLE_INPUT</td> |
| 1794 |
* <td class="ecr"> |
| 1795 |
* <i>self</i> and <i>perm</i> have incompatible sizes. |
| 1796 |
* </td> |
| 1797 |
* </tr> |
| 1798 |
* <tr> |
| 1799 |
* <td class="ecl">CPL_ERROR_TYPE_MISMATCH</td> |
| 1800 |
* <td class="ecr"> |
| 1801 |
* <i>perm</i> is not of type CPL_TYPE_INT. |
| 1802 |
* </td> |
| 1803 |
* </tr> |
| 1804 |
* <tr> |
| 1805 |
* <td class="ecl">CPL_ERROR_SINGULAR_MATRIX</td> |
| 1806 |
* <td class="ecr"> |
| 1807 |
* <i>self</i> is singular. |
| 1808 |
* </td> |
| 1809 |
* </tr> |
| 1810 |
* </table> |
| 1811 |
* @enderror |
| 1812 |
* |
| 1813 |
*/ |
| 1814 |
cpl_error_code cpl_matrix_decomp_lu(cpl_matrix * self, int * iperm, |
| 1815 |
int * psig) |
| 1816 |
{ |
| 1817 |
|
| 1818 |
const cpl_size n = cpl_matrix_get_ncol(self); |
| 1819 |
int nn; |
| 1820 |
cpl_size i, j; |
| 1821 |
const double * aread; |
| 1822 |
double * awrite; |
| 1823 |
|
| 1824 |
cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT); |
| 1825 |
cpl_ensure_code(iperm != NULL, CPL_ERROR_NULL_INPUT); |
| 1826 |
cpl_ensure_code(psig != NULL, CPL_ERROR_NULL_INPUT); |
| 1827 |
|
| 1828 |
cpl_ensure_code(self->nc == self->nr, CPL_ERROR_ILLEGAL_INPUT); |
| 1829 |
|
| 1830 |
nn = (int)n; |
| 1831 |
|
| 1832 |
cpl_ensure_code((cpl_size)nn == n, CPL_ERROR_ILLEGAL_INPUT); |
| 1833 |
|
| 1834 |
aread = awrite = self->m; |
| 1835 |
|
| 1836 |
/* No row permutations yet */ |
| 1837 |
*psig = 1; |
| 1838 |
for (i = 0; i < n; i++) { |
| 1839 |
iperm[i] = i; |
| 1840 |
} |
| 1841 |
|
| 1842 |
for (j = 0; j < n - 1; j++) { |
| 1843 |
/* Find maximum in the j-th column */ |
| 1844 |
|
| 1845 |
double pivot = fabs(aread[j + j * n]); |
| 1846 |
cpl_size ipivot = j; |
| 1847 |
|
| 1848 |
for (i = j + 1; i < n; i++) { |
| 1849 |
const double aij = fabs(aread[j + i * n]); |
| 1850 |
|
| 1851 |
if (aij > pivot) { |
| 1852 |
pivot = aij; |
| 1853 |
ipivot = i; |
| 1854 |
} |
| 1855 |
} |
| 1856 |
|
| 1857 |
if (pivot <= 0.0) { |
| 1858 |
return CPL_ERROR_SINGULAR_MATRIX; |
| 1859 |
} |
| 1860 |
|
| 1861 |
if (ipivot > j) { |
| 1862 |
/* The maximum is below the diagonal - swap rows */ |
| 1863 |
const int iswap = iperm[j]; |
| 1864 |
|
| 1865 |
iperm[j] = iperm[ipivot]; |
| 1866 |
iperm[ipivot] = iswap; |
| 1867 |
|
| 1868 |
*psig = - (*psig); |
| 1869 |
|
| 1870 |
swap_rows(self, j, ipivot); |
| 1871 |
} |
| 1872 |
|
| 1873 |
pivot = aread[j + j * n]; |
| 1874 |
|
| 1875 |
for (i = j + 1; i < n; i++) { |
| 1876 |
const double aij = aread[j + i * n] / pivot; |
| 1877 |
cpl_size k; |
| 1878 |
|
| 1879 |
awrite[j + i * n] = aij; |
| 1880 |
|
| 1881 |
for (k = j + 1; k < n; k++) { |
| 1882 |
const double ajk = aread[k + j * n]; |
| 1883 |
const double aik = aread[k + i * n]; |
| 1884 |
awrite[k + i * n] = aik - aij * ajk; |
| 1885 |
} |
| 1886 |
} |
| 1887 |
} |
| 1888 |
|
| 1889 |
/* Check if A(n,n) is non-zero */ |
| 1890 |
return fabs(aread[n * (n-1) + (n-1)]) > 0.0 ? CPL_ERROR_NONE |
| 1891 |
: CPL_ERROR_SINGULAR_MATRIX; |
| 1892 |
|
| 1893 |
} |
| 1894 |
|
| 1895 |
/* |
| 1896 |
* @brief Solve a LU-system |
| 1897 |
* @param self n X n LU-matrix from cpl_matrix_decomp_lu() |
| 1898 |
* @param rhs m right-hand-sides to be replaced by their solution |
| 1899 |
* @param perm n-integer array filled with the row permutations, or NULL |
| 1900 |
* @return CPL_ERROR_NONE on success, or the relevant CPL error code |
| 1901 |
* @see cpl_matrix_decomp_lu() |
| 1902 |
* |
| 1903 |
* @error |
| 1904 |
* <table class="ec" align="center"> |
| 1905 |
* <tr> |
| 1906 |
* <td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 1907 |
* <td class="ecr"> |
| 1908 |
* An input pointer is <tt>NULL</tt>. |
| 1909 |
* </td> |
| 1910 |
* </tr> |
| 1911 |
* <tr> |
| 1912 |
* <td class="ecl">CPL_ERROR_ILLEGAL_INPUT</td> |
| 1913 |
* <td class="ecr"> |
| 1914 |
* <i>self</i> is not an n by n matrix. |
| 1915 |
* </td> |
| 1916 |
* </tr> |
| 1917 |
* <tr> |
| 1918 |
* <td class="ecl">CPL_ERROR_INCOMPATIBLE_INPUT</td> |
| 1919 |
* <td class="ecr"> |
| 1920 |
* The array or matrices not have the same number of rows. |
| 1921 |
* </td> |
| 1922 |
* </tr> |
| 1923 |
* <tr> |
| 1924 |
* <td class="ecl">CPL_ERROR_TYPE_MISMATCH</td> |
| 1925 |
* <td class="ecr"> |
| 1926 |
* <i>perm</i> is non-NULL and not of type CPL_TYPE_INT. |
| 1927 |
* </td> |
| 1928 |
* </tr> |
| 1929 |
* <tr> |
| 1930 |
* <td class="ecl">CPL_ERROR_DIVISION_BY_ZERO</td> |
| 1931 |
* <td class="ecr"> |
| 1932 |
* The main diagonal of U contains a zero. This error can only occur |
| 1933 |
* if the LU-matrix does not come from a successful call to |
| 1934 |
* cpl_matrix_decomp_lu(). |
| 1935 |
* </td> |
| 1936 |
* </tr> |
| 1937 |
* </table> |
| 1938 |
* @enderror |
| 1939 |
* |
| 1940 |
*/ |
| 1941 |
cpl_error_code cpl_matrix_solve_lu(const cpl_matrix * self, |
| 1942 |
cpl_matrix * rhs, const int * iperm) |
| 1943 |
{ |
| 1944 |
|
| 1945 |
cpl_size n, i, j, k; |
| 1946 |
double * x = NULL; /* Avoid false uninit warning */ |
| 1947 |
const double * aread; |
| 1948 |
const double * bread; |
| 1949 |
double * bwrite; |
| 1950 |
|
| 1951 |
cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT); |
| 1952 |
cpl_ensure_code(rhs != NULL, CPL_ERROR_NULL_INPUT); |
| 1953 |
|
| 1954 |
cpl_ensure_code(self->nc == self->nr, CPL_ERROR_ILLEGAL_INPUT); |
| 1955 |
cpl_ensure_code(rhs->nr == self->nr, CPL_ERROR_INCOMPATIBLE_INPUT); |
| 1956 |
|
| 1957 |
n = self->nc; |
| 1958 |
|
| 1959 |
aread = self->m; |
| 1960 |
bread = bwrite = rhs->m; |
| 1961 |
|
| 1962 |
if (iperm == NULL) { |
| 1963 |
return CPL_ERROR_NULL_INPUT; |
| 1964 |
} |
| 1965 |
else { |
| 1966 |
x = (double*) cpl_malloc((size_t)n * sizeof(*x)); |
| 1967 |
} |
| 1968 |
|
| 1969 |
for (k=0; k < rhs->nc; k++) { |
| 1970 |
if (iperm != NULL) { |
| 1971 |
/* Un-permute the rows in column k */ |
| 1972 |
for (i = 0; i < n; i++) { |
| 1973 |
x[i] = bread[rhs->nc * i + k]; |
| 1974 |
} |
| 1975 |
for (i = 0; i < n; i++) { |
| 1976 |
bwrite[rhs->nc * i + k] = x[iperm[i]]; |
| 1977 |
} |
| 1978 |
} |
| 1979 |
|
| 1980 |
/* Forward substitution in column k */ |
| 1981 |
for (i = 1; i < n; i++) { |
| 1982 |
double tmp = bread[rhs->nc * i + k]; |
| 1983 |
for (j = 0; j < i; j++) { |
| 1984 |
const double aij = aread[n * i + j]; |
| 1985 |
|
| 1986 |
tmp -= aij * bread[rhs->nc * j + k]; |
| 1987 |
} |
| 1988 |
bwrite[rhs->nc * i + k] = tmp; |
| 1989 |
} |
| 1990 |
|
| 1991 |
/* Back substitution in column k */ |
| 1992 |
for (i = n - 1; i >= 0; i--) { |
| 1993 |
double tmp = bread[rhs->nc * i + k]; |
| 1994 |
for (j = i + 1; j < n; j++) { |
| 1995 |
const double aij = aread[n * i + j]; |
| 1996 |
|
| 1997 |
tmp -= aij * bread[rhs->nc * j + k]; |
| 1998 |
} |
| 1999 |
|
| 2000 |
/* Check for a bug in the calling function */ |
| 2001 |
if (aread[n * i + i] == 0.0) break; |
| 2002 |
|
| 2003 |
bwrite[rhs->nc * i + k] = tmp/aread[n * i + i]; |
| 2004 |
|
| 2005 |
} |
| 2006 |
|
| 2007 |
if (i >= 0) break; |
| 2008 |
} |
| 2009 |
|
| 2010 |
|
| 2011 |
return k == rhs->nc ? CPL_ERROR_NONE |
| 2012 |
: cpl_error_set_(CPL_ERROR_DIVISION_BY_ZERO); |
| 2013 |
|
| 2014 |
} |
| 2015 |
|
| 2016 |
|
| 2017 |
/*----------------------------------------------------------------------------*/ |
| 2018 |
/** |
| 2019 |
@brief Replace a matrix by its Cholesky-decomposition, L * transpose(L) = A |
| 2020 |
@param self N by N symmetric positive-definite matrix to decompose |
| 2021 |
@return CPL_ERROR_NONE on success, or the relevant CPL error code |
| 2022 |
@note Only the upper triangle of self is read, L is written in the lower |
| 2023 |
triangle |
| 2024 |
@note If the matrix is singular the elements of self become undefined |
| 2025 |
@see Golub & Van Loan, Matrix Computations, Algorithm 4.2.1 |
| 2026 |
(Cholesky: Gaxpy Version). |
| 2027 |
|
| 2028 |
@error |
| 2029 |
<table class="ec" align="center"> |
| 2030 |
<tr> |
| 2031 |
<td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 2032 |
<td class="ecr"> |
| 2033 |
An input pointer is <tt>NULL</tt>. |
| 2034 |
</td> |
| 2035 |
</tr> |
| 2036 |
<tr> |
| 2037 |
<td class="ecl">CPL_ERROR_ILLEGAL_INPUT</td> |
| 2038 |
<td class="ecr"> |
| 2039 |
<i>self</i> is not an n by n matrix. |
| 2040 |
</td> |
| 2041 |
</tr> |
| 2042 |
<tr> |
| 2043 |
<td class="ecl">CPL_ERROR_SINGULAR_MATRIX</td> |
| 2044 |
<td class="ecr"> |
| 2045 |
<i>self</i> is not symmetric, positive definite. |
| 2046 |
</td> |
| 2047 |
</tr> |
| 2048 |
</table> |
| 2049 |
@enderror |
| 2050 |
*/ |
| 2051 |
/*----------------------------------------------------------------------------*/ |
| 2052 |
cpl_error_code cpl_matrix_decomp_chol(cpl_matrix * self) |
| 2053 |
{ |
| 2054 |
|
| 2055 |
const cpl_size n = cpl_matrix_get_ncol(self); |
| 2056 |
double * awrite = cpl_matrix_get_data(self); |
| 2057 |
const double * aread = awrite; |
| 2058 |
double sub = 0.0; /* Fix (false) uninit warning */ |
| 2059 |
cpl_size i, j = 0; /* Fix (false) uninit warning */ |
| 2060 |
|
| 2061 |
cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT); |
| 2062 |
cpl_ensure_code(cpl_matrix_get_nrow(self) == n, CPL_ERROR_ILLEGAL_INPUT); |
| 2063 |
|
| 2064 |
for (i = 0; i < n; i++) { |
| 2065 |
for (j = i; j < n; j++) { |
| 2066 |
cpl_size k; |
| 2067 |
|
| 2068 |
sub = aread[n * i + j]; |
| 2069 |
for (k = i-1; k >= 0; k--) { |
| 2070 |
sub -= aread[n * i + k] * aread[n * j + k]; |
| 2071 |
} |
| 2072 |
|
| 2073 |
if (j > i) { |
| 2074 |
awrite[n * j + i] = sub/aread[n * i + i]; |
| 2075 |
} else if (sub > 0.0) { |
| 2076 |
awrite[n * i + i] = sqrt(sub); |
| 2077 |
} else { |
| 2078 |
break; |
| 2079 |
} |
| 2080 |
} |
| 2081 |
if (j < n) break; |
| 2082 |
} |
| 2083 |
|
| 2084 |
|
| 2085 |
return i == n ? CPL_ERROR_NONE |
| 2086 |
: CPL_ERROR_SINGULAR_MATRIX; |
| 2087 |
} |
| 2088 |
|
| 2089 |
|
| 2090 |
|
| 2091 |
/*----------------------------------------------------------------------------*/ |
| 2092 |
/** |
| 2093 |
@brief Solve a L*transpose(L)-system |
| 2094 |
@param self N by N L*transpose(L)-matrix from cpl_matrix_decomp_chol() |
| 2095 |
@param rhs M right-hand-sides to be replaced by their solution |
| 2096 |
@return CPL_ERROR_NONE on success, or the relevant CPL error code |
| 2097 |
@see cpl_matrix_decomp_chol() |
| 2098 |
@note Only the lower triangle of self is accessed |
| 2099 |
|
| 2100 |
@error |
| 2101 |
<table class="ec" align="center"> |
| 2102 |
<tr> |
| 2103 |
<td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 2104 |
<td class="ecr"> |
| 2105 |
An input pointer is <tt>NULL</tt>. |
| 2106 |
</td> |
| 2107 |
</tr> |
| 2108 |
<tr> |
| 2109 |
<td class="ecl">CPL_ERROR_ILLEGAL_INPUT</td> |
| 2110 |
<td class="ecr"> |
| 2111 |
<i>self</i> is not an n by n matrix. |
| 2112 |
</td> |
| 2113 |
</tr> |
| 2114 |
<tr> |
| 2115 |
<td class="ecl">CPL_ERROR_INCOMPATIBLE_INPUT</td> |
| 2116 |
<td class="ecr"> |
| 2117 |
The specified matrices do not have the same number of rows. |
| 2118 |
</td> |
| 2119 |
</tr> |
| 2120 |
<tr> |
| 2121 |
<td class="ecl">CPL_ERROR_DIVISION_BY_ZERO</td> |
| 2122 |
<td class="ecr"> |
| 2123 |
The main diagonal of L contains a zero. This error can only occur |
| 2124 |
if the L*transpose(L)-matrix does not come from a successful call to |
| 2125 |
cpl_matrix_decomp_chol(). |
| 2126 |
</td> |
| 2127 |
</tr> |
| 2128 |
</table> |
| 2129 |
@enderror |
| 2130 |
*/ |
| 2131 |
/*----------------------------------------------------------------------------*/ |
| 2132 |
cpl_error_code cpl_matrix_solve_chol(const cpl_matrix * self, |
| 2133 |
cpl_matrix * rhs) |
| 2134 |
{ |
| 2135 |
|
| 2136 |
const cpl_size n = cpl_matrix_get_ncol(self); |
| 2137 |
const cpl_size nrhs = cpl_matrix_get_ncol(rhs); |
| 2138 |
cpl_size i, j, k; |
| 2139 |
const double * aread; |
| 2140 |
const double * bread; |
| 2141 |
double * bwrite; |
| 2142 |
|
| 2143 |
cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT); |
| 2144 |
cpl_ensure_code(rhs != NULL, CPL_ERROR_NULL_INPUT); |
| 2145 |
|
| 2146 |
cpl_ensure_code(cpl_matrix_get_nrow(self) == n, CPL_ERROR_ILLEGAL_INPUT); |
| 2147 |
cpl_ensure_code(cpl_matrix_get_nrow(rhs) == n, CPL_ERROR_INCOMPATIBLE_INPUT); |
| 2148 |
|
| 2149 |
aread = cpl_matrix_get_data_const(self); |
| 2150 |
bread = bwrite = cpl_matrix_get_data(rhs); |
| 2151 |
|
| 2152 |
for (k=0; k < nrhs; k++) { |
| 2153 |
/* Forward substitution in column k */ |
| 2154 |
for (i = 0; i < n; i++) { |
| 2155 |
double sub = bread[nrhs * i + k]; |
| 2156 |
for (j = i-1; j >= 0; j--) { |
| 2157 |
sub -= aread[n * i + j] * bread[nrhs * j + k]; |
| 2158 |
} |
| 2159 |
cpl_ensure_code(aread[n * i + i] != 0.0, |
| 2160 |
CPL_ERROR_DIVISION_BY_ZERO); |
| 2161 |
bwrite[nrhs * i + k] = sub/aread[n * i + i]; |
| 2162 |
} |
| 2163 |
|
| 2164 |
/* Back substitution in column k */ |
| 2165 |
|
| 2166 |
for (i = n-1; i >= 0; i--) { |
| 2167 |
double sub = bread[nrhs * i + k]; |
| 2168 |
for (j = i+1; j < n; j++) { |
| 2169 |
sub -= aread[n * j + i] * bread[nrhs * j + k]; |
| 2170 |
} |
| 2171 |
bwrite[nrhs * i + k] = sub/aread[n * i + i]; |
| 2172 |
} |
| 2173 |
} |
| 2174 |
|
| 2175 |
return CPL_ERROR_NONE; |
| 2176 |
|
| 2177 |
} |
| 2178 |
|
| 2179 |
|
| 2180 |
|
| 2181 |
|
| 2182 |
/** |
| 2183 |
* @brief |
| 2184 |
* Solution of a linear system. |
| 2185 |
* |
| 2186 |
* @param coeff A non-singular N by N matrix. |
| 2187 |
* @param rhs A matrix containing one or more right-hand-sides. |
| 2188 |
* @note rhs must have N rows and may contain more than one column, |
| 2189 |
* which each represent an independent right-hand-side. |
| 2190 |
* |
| 2191 |
* @return A newly allocated solution matrix with the size as rhs, |
| 2192 |
* or @c NULL on error. |
| 2193 |
* |
| 2194 |
* @error |
| 2195 |
* <table class="ec" align="center"> |
| 2196 |
* <tr> |
| 2197 |
* <td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 2198 |
* <td class="ecr"> |
| 2199 |
* Any input is a <tt>NULL</tt> pointer. |
| 2200 |
* </td> |
| 2201 |
* </tr> |
| 2202 |
* <tr> |
| 2203 |
* <td class="ecl">CPL_ERROR_ILLEGAL_INPUT</td> |
| 2204 |
* <td class="ecr"> |
| 2205 |
* <i>coeff</i> is not a square matrix. |
| 2206 |
* </td> |
| 2207 |
* </tr> |
| 2208 |
* <tr> |
| 2209 |
* <td class="ecl">CPL_ERROR_INCOMPATIBLE_INPUT</td> |
| 2210 |
* <td class="ecr"> |
| 2211 |
* <i>coeff</i> and <i>rhs</i> do not have the same number of rows. |
| 2212 |
* </td> |
| 2213 |
* </tr> |
| 2214 |
* <tr> |
| 2215 |
* <td class="ecl">CPL_ERROR_SINGULAR_MATRIX</td> |
| 2216 |
* <td class="ecr"> |
| 2217 |
* <i>coeff</i> is singular (to working precision). |
| 2218 |
* </td> |
| 2219 |
* </tr> |
| 2220 |
* </table> |
| 2221 |
* @enderror |
| 2222 |
* |
| 2223 |
* Compute the solution of a system of N equations with N unknowns: |
| 2224 |
* |
| 2225 |
* coeff * X = rhs |
| 2226 |
* |
| 2227 |
* @em coeff must be an NxN matrix, and @em rhs a NxM matrix. |
| 2228 |
* M greater than 1 means that multiple independent right-hand-sides |
| 2229 |
* are solved for. |
| 2230 |
* To destroy the solution matrix the function @c cpl_matrix_delete() |
| 2231 |
* should be used. |
| 2232 |
* |
| 2233 |
*/ |
| 2234 |
|
| 2235 |
cpl_matrix *cpl_matrix_solve(const cpl_matrix *coeff, const cpl_matrix *rhs) |
| 2236 |
{ |
| 2237 |
|
| 2238 |
cpl_matrix * lu; |
| 2239 |
cpl_matrix * x; |
| 2240 |
int * iperm; |
| 2241 |
cpl_size n; |
| 2242 |
int sig; |
| 2243 |
cpl_error_code error; |
| 2244 |
|
| 2245 |
|
| 2246 |
cpl_ensure(coeff != NULL, CPL_ERROR_NULL_INPUT, NULL); |
| 2247 |
cpl_ensure(rhs != NULL, CPL_ERROR_NULL_INPUT, NULL); |
| 2248 |
|
| 2249 |
n = coeff->nc; |
| 2250 |
|
| 2251 |
cpl_ensure(coeff->nr == n, CPL_ERROR_ILLEGAL_INPUT, NULL); |
| 2252 |
cpl_ensure(rhs->nr == n, CPL_ERROR_INCOMPATIBLE_INPUT, NULL); |
| 2253 |
|
| 2254 |
lu = cpl_matrix_duplicate(coeff); |
| 2255 |
|
| 2256 |
iperm = (int*) cpl_malloc((size_t)n * sizeof(*iperm)); |
| 2257 |
|
| 2258 |
if (cpl_matrix_decomp_lu(lu, iperm, &sig)) { |
| 2259 |
cpl_matrix_delete(lu); |
| 2260 |
cpl_free(iperm); |
| 2261 |
(void)cpl_error_set_where_(); |
| 2262 |
return NULL; |
| 2263 |
} |
| 2264 |
|
| 2265 |
x = cpl_matrix_duplicate(rhs); |
| 2266 |
|
| 2267 |
/* should not be able to fail at this point */ |
| 2268 |
error = cpl_matrix_solve_lu(lu, x, iperm); |
| 2269 |
|
| 2270 |
cpl_matrix_delete(lu); |
| 2271 |
cpl_free(iperm); |
| 2272 |
|
| 2273 |
if (error) { |
| 2274 |
cpl_matrix_delete(x); |
| 2275 |
x = NULL; |
| 2276 |
(void)cpl_error_set_where_(); |
| 2277 |
} |
| 2278 |
|
| 2279 |
return x; |
| 2280 |
} |
| 2281 |
|
| 2282 |
|
| 2283 |
/** |
| 2284 |
* @brief |
| 2285 |
* Solution of overdetermined linear equations in a least squares sense. |
| 2286 |
* |
| 2287 |
* @param coeff The N by M matrix of coefficients, where N >= M. |
| 2288 |
* @param rhs An N by K matrix containing K right-hand-sides. |
| 2289 |
* @note rhs may contain more than one column, |
| 2290 |
* which each represent an independent right-hand-side. |
| 2291 |
* |
| 2292 |
* @return A newly allocated M by K solution matrix, |
| 2293 |
* or @c NULL on error. |
| 2294 |
* |
| 2295 |
* @error |
| 2296 |
* <table class="ec" align="center"> |
| 2297 |
* <tr> |
| 2298 |
* <td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 2299 |
* <td class="ecr"> |
| 2300 |
* Any input is a <tt>NULL</tt> pointer. |
| 2301 |
* </td> |
| 2302 |
* </tr> |
| 2303 |
* <tr> |
| 2304 |
* <td class="ecl">CPL_ERROR_INCOMPATIBLE_INPUT</td> |
| 2305 |
* <td class="ecr"> |
| 2306 |
* <i>coeff</i> and <i>rhs</i> do not have the same number of rows. |
| 2307 |
* </td> |
| 2308 |
* </tr> |
| 2309 |
* <tr> |
| 2310 |
* <td class="ecl">CPL_ERROR_SINGULAR_MATRIX</td> |
| 2311 |
* <td class="ecr"> |
| 2312 |
* The matrix is (near) singular and a solution cannot be computed. |
| 2313 |
* </td> |
| 2314 |
* </tr> |
| 2315 |
* </table> |
| 2316 |
* @enderror |
| 2317 |
* |
| 2318 |
* The following linear system of N equations and M unknowns |
| 2319 |
* is given: |
| 2320 |
* |
| 2321 |
* @code |
| 2322 |
* coeff * X = rhs |
| 2323 |
* @endcode |
| 2324 |
* |
| 2325 |
* where @em coeff is the NxM matrix of the coefficients, @em X is the |
| 2326 |
* MxK matrix of the unknowns, and @em rhs the NxK matrix containing |
| 2327 |
* the K right hand side(s). |
| 2328 |
* |
| 2329 |
* The solution to the normal equations is known to be a least-squares |
| 2330 |
* solution, i.e. the 2-norm of coeff * X - rhs is minimized by the |
| 2331 |
* solution to |
| 2332 |
* transpose(coeff) * coeff * X = transpose(coeff) * rhs. |
| 2333 |
* |
| 2334 |
* In the case that coeff is square (N is equal to M) it gives a faster |
| 2335 |
* and more accurate result to use cpl_matrix_solve(). |
| 2336 |
* |
| 2337 |
* The solution matrix should be deallocated with the function |
| 2338 |
* @c cpl_matrix_delete(). |
| 2339 |
*/ |
| 2340 |
|
| 2341 |
cpl_matrix *cpl_matrix_solve_normal(const cpl_matrix *coeff, |
| 2342 |
const cpl_matrix *rhs) |
| 2343 |
{ |
| 2344 |
|
| 2345 |
cpl_matrix * solution; |
| 2346 |
cpl_matrix * At; |
| 2347 |
cpl_matrix * AtA; |
| 2348 |
|
| 2349 |
cpl_ensure(coeff != NULL, CPL_ERROR_NULL_INPUT, NULL); |
| 2350 |
cpl_ensure(rhs != NULL, CPL_ERROR_NULL_INPUT, NULL); |
| 2351 |
cpl_ensure(rhs->nr == coeff->nr, CPL_ERROR_INCOMPATIBLE_INPUT, NULL); |
| 2352 |
|
| 2353 |
At = cpl_matrix_transpose_create(coeff); |
| 2354 |
solution = cpl_matrix_product_create(At, rhs); |
| 2355 |
|
| 2356 |
AtA = cpl_matrix_product_normal_create(At); |
| 2357 |
|
| 2358 |
cpl_matrix_delete(At); |
| 2359 |
|
| 2360 |
if (cpl_matrix_solve_spd(AtA, solution)) { |
| 2361 |
cpl_matrix_delete(solution); |
| 2362 |
solution = NULL; |
| 2363 |
(void)cpl_error_set_where_(); |
| 2364 |
} |
| 2365 |
|
| 2366 |
cpl_matrix_delete(AtA); |
| 2367 |
|
| 2368 |
return solution; |
| 2369 |
|
| 2370 |
} |
| 2371 |
|
| 2372 |
|
| 2373 |
/*----------------------------------------------------------------------------*/ |
| 2374 |
/** |
| 2375 |
@internal |
| 2376 |
@brief |
| 2377 |
Solution of Symmetric, Positive Definite system of linear equations. |
| 2378 |
* |
| 2379 |
@param self The N by N Symmetric, Positive Definite matrix. |
| 2380 |
@param rhs An N by K matrix containing K right-hand-sides. |
| 2381 |
@note rhs may contain more than one column, |
| 2382 |
which each represent an independent right-hand-side. The solution |
| 2383 |
is written in rhs. Only the upper triangular part of self is read, |
| 2384 |
while the lower triangular part is modified. |
| 2385 |
@return CPL_ERROR_NONE on success, or the relevant CPL error code |
| 2386 |
* |
| 2387 |
@error |
| 2388 |
<table class="ec" align="center"> |
| 2389 |
<tr> |
| 2390 |
<td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 2391 |
<td class="ecr"> |
| 2392 |
<i>self</i> or <i>rhs</i> is a <tt>NULL</tt> pointer. |
| 2393 |
</td> |
| 2394 |
</tr> |
| 2395 |
<tr> |
| 2396 |
<td class="ecl">CPL_ERROR_INCOMPATIBLE_INPUT</td> |
| 2397 |
<td class="ecr"> |
| 2398 |
<i>self</i> and <i>rhs</i> do not have the same number of rows. |
| 2399 |
</td> |
| 2400 |
</tr> |
| 2401 |
<tr> |
| 2402 |
<td class="ecl">CPL_ERROR_ILLEGAL_INPUT</td> |
| 2403 |
<td class="ecr"> |
| 2404 |
<i>self</i> is not an N by N matrix. |
| 2405 |
</td> |
| 2406 |
</tr> |
| 2407 |
<tr> |
| 2408 |
<td class="ecl">CPL_ERROR_SINGULAR_MATRIX</td> |
| 2409 |
<td class="ecr"> |
| 2410 |
<i>self</i> is (near) singular and a solution cannot be computed. |
| 2411 |
</td> |
| 2412 |
</tr> |
| 2413 |
</table> |
| 2414 |
@enderror |
| 2415 |
*/ |
| 2416 |
/*----------------------------------------------------------------------------*/ |
| 2417 |
|
| 2418 |
cpl_error_code cpl_matrix_solve_spd(cpl_matrix *self, |
| 2419 |
cpl_matrix *rhs) |
| 2420 |
{ |
| 2421 |
|
| 2422 |
cpl_ensure_code(!cpl_matrix_decomp_chol(self), cpl_error_get_code()); |
| 2423 |
|
| 2424 |
cpl_ensure_code(!cpl_matrix_solve_chol(self, rhs), cpl_error_get_code()); |
| 2425 |
|
| 2426 |
return CPL_ERROR_NONE; |
| 2427 |
|
| 2428 |
} |
| 2429 |
|
| 2430 |
|
| 2431 |
/** |
| 2432 |
* @internal |
| 2433 |
* @brief Compute A = B * transpose(B) |
| 2434 |
* |
| 2435 |
* @param self Pre-allocated M x M matrix to hold the result |
| 2436 |
* @param other M x N Matrix to multiply with its transpose |
| 2437 |
@return CPL_ERROR_NONE or the relevant CPL error code on error |
| 2438 |
* @note Only the upper triangle of A is computed, while the elements |
| 2439 |
* below the main diagonal have undefined values. |
| 2440 |
* @see cpl_matrix_product_create() |
| 2441 |
* |
| 2442 |
* @error |
| 2443 |
* <table class="ec" align="center"> |
| 2444 |
* <tr> |
| 2445 |
* <td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 2446 |
* <td class="ecr"> |
| 2447 |
* An input matrix is a <tt>NULL</tt> pointer. |
| 2448 |
* </td> |
| 2449 |
* </tr> |
| 2450 |
* <tr> |
| 2451 |
* <td class="ecl">CPL_ERROR_ILLEGAL_INPUT</td> |
| 2452 |
* <td class="ecr"> |
| 2453 |
* <i>self</i> is not an M by M matrix. |
| 2454 |
* </td> |
| 2455 |
* </tr> |
| 2456 |
* <tr> |
| 2457 |
* <td class="ecl">CPL_ERROR_INCOMPATIBLE_INPUT</td> |
| 2458 |
* <td class="ecr"> |
| 2459 |
* The two matrices have a different number of rows. |
| 2460 |
* </td> |
| 2461 |
* </tr> |
| 2462 |
* <tr> |
| 2463 |
* |
| 2464 |
* </table> |
| 2465 |
* @enderror |
| 2466 |
* |
| 2467 |
*/ |
| 2468 |
|
| 2469 |
cpl_error_code cpl_matrix_product_normal(cpl_matrix * self, |
| 2470 |
const cpl_matrix * other) |
| 2471 |
{ |
| 2472 |
|
| 2473 |
double sum; |
| 2474 |
const double * ai = cpl_matrix_get_data_const(other); |
| 2475 |
const double * aj; |
| 2476 |
double * bwrite = cpl_matrix_get_data(self); |
| 2477 |
const cpl_size m = cpl_matrix_get_nrow(self); |
| 2478 |
const cpl_size n = cpl_matrix_get_ncol(other); |
| 2479 |
cpl_size i, j, k; |
| 2480 |
|
| 2481 |
|
| 2482 |
cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT); |
| 2483 |
cpl_ensure_code(other != NULL, CPL_ERROR_NULL_INPUT); |
| 2484 |
cpl_ensure_code(cpl_matrix_get_ncol(self) == m, CPL_ERROR_ILLEGAL_INPUT); |
| 2485 |
cpl_ensure_code(cpl_matrix_get_nrow(other) == m, |
| 2486 |
CPL_ERROR_INCOMPATIBLE_INPUT); |
| 2487 |
|
| 2488 |
#ifdef CPL_MATRIX_PRODUCT_NORMAL_RESET |
| 2489 |
/* Initialize all values to zero. |
| 2490 |
This is done to avoid access of uninitilized memory in case |
| 2491 |
someone passes the matrix to for example cpl_matrix_dump(). */ |
| 2492 |
(void)memset(bwrite, 0, m * n * sizeof(*bwrite)); |
| 2493 |
#endif |
| 2494 |
|
| 2495 |
/* The result at (i,j) is the dot-product of i'th and j'th row */ |
| 2496 |
for (i = 0; i < m; i++, bwrite += m, ai += n) { |
| 2497 |
aj = ai; /* aj points to first entry in j'th row */ |
| 2498 |
for (j = i; j < m; j++, aj += n) { |
| 2499 |
sum = 0.0; |
| 2500 |
for (k = 0; k < n; k++) { |
| 2501 |
sum += ai[k] * aj[k]; |
| 2502 |
} |
| 2503 |
bwrite[j] = sum; |
| 2504 |
} |
| 2505 |
} |
| 2506 |
|
| 2507 |
return CPL_ERROR_NONE; |
| 2508 |
} |
| 2509 |
|
| 2510 |
|
| 2511 |
/** |
| 2512 |
* @internal |
| 2513 |
* @brief Create and compute A = B * transpose(B) |
| 2514 |
* |
| 2515 |
* @param self M x N Matrix |
| 2516 |
* @return Pointer to created M x M product matrix, or @c NULL on error. |
| 2517 |
* @note Only the upper triangle of A is computed, while the elements |
| 2518 |
* below the main diagonal have undefined values. |
| 2519 |
* @see cpl_matrix_product_create() |
| 2520 |
* |
| 2521 |
* @error |
| 2522 |
* <table class="ec" align="center"> |
| 2523 |
* <tr> |
| 2524 |
* <td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 2525 |
* <td class="ecr"> |
| 2526 |
* An input matrix is a <tt>NULL</tt> pointer. |
| 2527 |
* </td> |
| 2528 |
* </tr> |
| 2529 |
* </table> |
| 2530 |
* @enderror |
| 2531 |
* |
| 2532 |
* To destroy the new matrix the function @c cpl_matrix_delete() should |
| 2533 |
* be used. |
| 2534 |
*/ |
| 2535 |
|
| 2536 |
cpl_matrix * cpl_matrix_product_normal_create(const cpl_matrix * self) |
| 2537 |
{ |
| 2538 |
|
| 2539 |
const size_t m = cpl_matrix_get_nrow(self); |
| 2540 |
cpl_matrix * product = cpl_matrix_wrap((cpl_size)m, (cpl_size)m, |
| 2541 |
cpl_malloc(m * m * sizeof(double))); |
| 2542 |
|
| 2543 |
if (cpl_matrix_product_normal(product, self)) { |
| 2544 |
cpl_matrix_delete(product); |
| 2545 |
product = NULL; |
| 2546 |
(void)cpl_error_set_where_(); |
| 2547 |
} |
| 2548 |
|
| 2549 |
return product; |
| 2550 |
} |
| 2551 |
|
| 2552 |
/*----------------------------------------------------------------------------*/ |
| 2553 |
/** |
| 2554 |
@internal |
| 2555 |
@brief Fill a matrix with the product of A * B |
| 2556 |
@param self The matrix to fill, is or else will be set to size M x N |
| 2557 |
@param ma The matrix A, of size M x K |
| 2558 |
@param mb The matrix B, of size K * N |
| 2559 |
@return CPL_ERROR_NONE or the relevant CPL error code on error |
| 2560 |
@note If upon entry, self has a size m *n, which differs from its size on |
| 2561 |
return, then the result of preceeding calls to cpl_matrix_get_data() |
| 2562 |
are invalid. |
| 2563 |
@see cpl_matrix_product_create() |
| 2564 |
|
| 2565 |
*/ |
| 2566 |
/*----------------------------------------------------------------------------*/ |
| 2567 |
|
| 2568 |
cpl_error_code cpl_matrix_product(cpl_matrix * self, |
| 2569 |
const cpl_matrix * ma, |
| 2570 |
const cpl_matrix * mb) |
| 2571 |
{ |
| 2572 |
const size_t bs = 48; |
| 2573 |
double * ds; |
| 2574 |
const double * d1 = cpl_matrix_get_data_const(ma); |
| 2575 |
const double * d2 = cpl_matrix_get_data_const(mb); |
| 2576 |
|
| 2577 |
const size_t nr = cpl_matrix_get_nrow(ma); |
| 2578 |
const size_t nc = cpl_matrix_get_ncol(mb); |
| 2579 |
const size_t nk = cpl_matrix_get_nrow(mb); |
| 2580 |
size_t i, j, k, ib, jb, kb; |
| 2581 |
|
| 2582 |
cpl_ensure_code(ma != NULL, CPL_ERROR_NULL_INPUT); |
| 2583 |
cpl_ensure_code(mb != NULL, CPL_ERROR_NULL_INPUT); |
| 2584 |
cpl_ensure_code((size_t)ma->nc == nk, CPL_ERROR_INCOMPATIBLE_INPUT); |
| 2585 |
|
| 2586 |
if (cpl_matrix_set_size_(self, nr, nc)) return cpl_error_set_where_(); |
| 2587 |
|
| 2588 |
ds = cpl_matrix_get_data(self); |
| 2589 |
|
| 2590 |
/* simple blocking to reduce cache misses */ |
| 2591 |
for (i = 0; i < nr; i += bs) { |
| 2592 |
for (j = 0; j < nc; j += bs) { |
| 2593 |
for (k = 0; k < nk; k += bs) { |
| 2594 |
for (ib = i; ib < CX_MIN(i + bs, nr); ib++) { |
| 2595 |
for (jb = j; jb < CX_MIN(j + bs, nc); jb++) { |
| 2596 |
double sum = 0.; |
| 2597 |
for (kb = k; kb < CX_MIN(k + bs, nk); kb++) { |
| 2598 |
sum += d1[ib * nk + kb] * d2[kb * nc + jb]; |
| 2599 |
} |
| 2600 |
ds[ib * nc + jb] += sum; |
| 2601 |
} |
| 2602 |
} |
| 2603 |
} |
| 2604 |
} |
| 2605 |
} |
| 2606 |
|
| 2607 |
|
| 2608 |
|
| 2609 |
return CPL_ERROR_NONE; |
| 2610 |
|
| 2611 |
} |
| 2612 |
|
| 2613 |
/*----------------------------------------------------------------------------*/ |
| 2614 |
/** |
| 2615 |
@internal |
| 2616 |
@brief Fill a matrix with the product of A * B' |
| 2617 |
@param self The matrix to fill, is or else will be set to size M x N |
| 2618 |
@param ma The matrix A, of size M x K |
| 2619 |
@param mb The matrix B, of size N x K |
| 2620 |
@return CPL_ERROR_NONE or the relevant CPL error code on error |
| 2621 |
@note The use of the transpose of B causes a more efficient memory access |
| 2622 |
@note Changing the order of A and B is allowed, it transposes the result |
| 2623 |
@see cpl_matrix_product_create() |
| 2624 |
|
| 2625 |
*/ |
| 2626 |
/*----------------------------------------------------------------------------*/ |
| 2627 |
cpl_error_code cpl_matrix_product_transpose(cpl_matrix * self, |
| 2628 |
const cpl_matrix * ma, |
| 2629 |
const cpl_matrix * mb) |
| 2630 |
{ |
| 2631 |
|
| 2632 |
double sum; |
| 2633 |
|
| 2634 |
double * ds; |
| 2635 |
const double * d1 = cpl_matrix_get_data_const(ma); |
| 2636 |
const double * d2 = cpl_matrix_get_data_const(mb); |
| 2637 |
const double * di; |
| 2638 |
|
| 2639 |
const cpl_size nr = cpl_matrix_get_nrow(ma); |
| 2640 |
const cpl_size nc = cpl_matrix_get_nrow(mb); |
| 2641 |
const cpl_size nk = cpl_matrix_get_ncol(mb); |
| 2642 |
cpl_size i, j, k; |
| 2643 |
|
| 2644 |
|
| 2645 |
cpl_ensure_code(ma != NULL, CPL_ERROR_NULL_INPUT); |
| 2646 |
cpl_ensure_code(mb != NULL, CPL_ERROR_NULL_INPUT); |
| 2647 |
cpl_ensure_code(ma->nc == nk, CPL_ERROR_INCOMPATIBLE_INPUT); |
| 2648 |
|
| 2649 |
if (cpl_matrix_set_size_(self, nr, nc)) return cpl_error_set_where_(); |
| 2650 |
|
| 2651 |
ds = cpl_matrix_get_data(self); |
| 2652 |
|
| 2653 |
for (i = 0; i < nr; i++, d1 += nk) { |
| 2654 |
/* Since ma and mb are addressed in the same manner, |
| 2655 |
they can use the same index, k */ |
| 2656 |
|
| 2657 |
di = d2; /* di points to first entry in i'th row */ |
| 2658 |
for (j = 0; j < nc; j++, di += nk) { |
| 2659 |
sum = 0.0; |
| 2660 |
for (k = 0; k < nk; k++) { |
| 2661 |
sum += d1[k] * di[k]; |
| 2662 |
} |
| 2663 |
ds[nc * i + j] = sum; |
| 2664 |
} |
| 2665 |
} |
| 2666 |
|
| 2667 |
return CPL_ERROR_NONE; |
| 2668 |
|
| 2669 |
} |
| 2670 |
|
| 2671 |
/*----------------------------------------------------------------------------*/ |
| 2672 |
/** |
| 2673 |
@internal |
| 2674 |
@brief Fill a matrix with the product of B * A * B' |
| 2675 |
@param self The matrix to fill, is or else will be set to size N x N |
| 2676 |
@param ma The matrix A, of size M x M |
| 2677 |
@param mb The matrix B, of size N x M |
| 2678 |
@return CPL_ERROR_NONE or the relevant CPL error code on error |
| 2679 |
@note Requires 2 * N * M * (N + M) FLOPs and M doubles of temporary storage |
| 2680 |
@see cpl_matrix_product_create() |
| 2681 |
|
| 2682 |
The intermediate result, C = A * B', is computed one column at a time, |
| 2683 |
each column of C is then used once to compute one row of the final result. |
| 2684 |
|
| 2685 |
The performed sequence of floating point operations is exactly the same as |
| 2686 |
in the combination of the two calls |
| 2687 |
cpl_matrix_product_transpose(C, A, B); |
| 2688 |
cpl_matrix_product_create(B, C); |
| 2689 |
and the result is therefore also exactly identical. |
| 2690 |
|
| 2691 |
The difference lies in the reduced use of temporary storage and in the |
| 2692 |
unit stride access, both of which lead to better performance (for large |
| 2693 |
matrices). |
| 2694 |
|
| 2695 |
*/ |
| 2696 |
/*----------------------------------------------------------------------------*/ |
| 2697 |
cpl_error_code cpl_matrix_product_bilinear(cpl_matrix * self, |
| 2698 |
const cpl_matrix * ma, |
| 2699 |
const cpl_matrix * mb) |
| 2700 |
{ |
| 2701 |
|
| 2702 |
double * ds; |
| 2703 |
double * cj; /* Holds one column of A * B' */ |
| 2704 |
const double * d1 = cpl_matrix_get_data_const(ma); |
| 2705 |
const double * d2 = cpl_matrix_get_data_const(mb); |
| 2706 |
|
| 2707 |
const cpl_size nr = cpl_matrix_get_nrow(mb); |
| 2708 |
const cpl_size nc = cpl_matrix_get_ncol(mb); |
| 2709 |
cpl_size i, j, k; |
| 2710 |
|
| 2711 |
|
| 2712 |
cpl_ensure_code(ma != NULL, CPL_ERROR_NULL_INPUT); |
| 2713 |
cpl_ensure_code(mb != NULL, CPL_ERROR_NULL_INPUT); |
| 2714 |
cpl_ensure_code(ma->nr == nc, CPL_ERROR_INCOMPATIBLE_INPUT); |
| 2715 |
cpl_ensure_code(ma->nc == nc, CPL_ERROR_INCOMPATIBLE_INPUT); |
| 2716 |
|
| 2717 |
if (cpl_matrix_set_size_(self, nr, nr)) return cpl_error_set_where_(); |
| 2718 |
|
| 2719 |
ds = cpl_matrix_get_data(self); |
| 2720 |
cj = cpl_malloc((size_t)nc * sizeof(*cj)); |
| 2721 |
|
| 2722 |
for (j = 0; j < nr; j++) { |
| 2723 |
|
| 2724 |
/* First compute the jth column of C = A * B', whose kth element |
| 2725 |
is the dot-product of kth row of A and the jth row of B, |
| 2726 |
both of which are read with unit stride. */ |
| 2727 |
for (k = 0; k < nc; k++) { |
| 2728 |
double sum = 0.0; |
| 2729 |
for (i = 0; i < nc; i++) { |
| 2730 |
sum += d1[nc * k + i] * d2[nc * j + i]; |
| 2731 |
} |
| 2732 |
cj[k] = sum; |
| 2733 |
} |
| 2734 |
|
| 2735 |
/* Then compute (i, j)th element of the result, which |
| 2736 |
is the dot-product of the ith row of B and the jth column of C, |
| 2737 |
both of which are read with unit stride. */ |
| 2738 |
for (i = 0; i < nr; i++) { |
| 2739 |
double sum = 0.0; |
| 2740 |
|
| 2741 |
for (k = 0; k < nc; k++) { |
| 2742 |
sum += d2[nc * i + k] * cj[k]; |
| 2743 |
} |
| 2744 |
ds[nr * i + j] = sum; |
| 2745 |
} |
| 2746 |
} |
| 2747 |
|
| 2748 |
cpl_free(cj); |
| 2749 |
|
| 2750 |
return CPL_ERROR_NONE; |
| 2751 |
|
| 2752 |
} |
| 2753 |
|
| 2754 |
/*----------------------------------------------------------------------------*/ |
| 2755 |
/** |
| 2756 |
@internal |
| 2757 |
@brief Solve a L*transpose(L)-system with a transposed Right Hand Side |
| 2758 |
@param self N by N L*transpose(L)-matrix from cpl_matrix_decomp_chol() |
| 2759 |
@param rhs M right-hand-sides to be replaced by their solution |
| 2760 |
@return CPL_ERROR_NONE on success, or the relevant CPL error code |
| 2761 |
@see cpl_matrix_solve_chol() |
| 2762 |
@note Only the lower triangle of self is accessed |
| 2763 |
@note The use of the transpose of rhs causes a more efficient memory access |
| 2764 |
|
| 2765 |
@error |
| 2766 |
<table class="ec" align="center"> |
| 2767 |
<tr> |
| 2768 |
<td class="ecl">CPL_ERROR_NULL_INPUT</td> |
| 2769 |
<td class="ecr"> |
| 2770 |
An input pointer is <tt>NULL</tt>. |
| 2771 |
</td> |
| 2772 |
</tr> |
| 2773 |
<tr> |
| 2774 |
<td class="ecl">CPL_ERROR_ILLEGAL_INPUT</td> |
| 2775 |
<td class="ecr"> |
| 2776 |
<i>self</i> is not an n by n matrix. |
| 2777 |
</td> |
| 2778 |
</tr> |
| 2779 |
<tr> |
| 2780 |
<td class="ecl">CPL_ERROR_INCOMPATIBLE_INPUT</td> |
| 2781 |
<td class="ecr"> |
| 2782 |
Selfs number of rows differs from rhs' number of columns. |
| 2783 |
</td> |
| 2784 |
</tr> |
| 2785 |
<tr> |
| 2786 |
<td class="ecl">CPL_ERROR_DIVISION_BY_ZERO</td> |
| 2787 |
<td class="ecr"> |
| 2788 |
The main diagonal of L contains a zero. This error can only occur |
| 2789 |
if the L*transpose(L)-matrix does not come from a successful call to |
| 2790 |
cpl_matrix_decomp_chol(). |
| 2791 |
</td> |
| 2792 |
</tr> |
| 2793 |
</table> |
| 2794 |
@enderror |
| 2795 |
|
| 2796 |
*/ |
| 2797 |
/*----------------------------------------------------------------------------*/ |
| 2798 |
cpl_error_code cpl_matrix_solve_chol_transpose(const cpl_matrix * self, |
| 2799 |
cpl_matrix * rhs) |
| 2800 |
{ |
| 2801 |
|
| 2802 |
const cpl_size n = cpl_matrix_get_ncol(self); |
| 2803 |
const cpl_size nrhs = cpl_matrix_get_nrow(rhs); |
| 2804 |
cpl_size i, j, k; |
| 2805 |
const double * aread; |
| 2806 |
const double * ai; |
| 2807 |
double * bk; |
| 2808 |
double sub; |
| 2809 |
|
| 2810 |
cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT); |
| 2811 |
cpl_ensure_code(rhs != NULL, CPL_ERROR_NULL_INPUT); |
| 2812 |
|
| 2813 |
cpl_ensure_code(self->nr == n, CPL_ERROR_ILLEGAL_INPUT); |
| 2814 |
cpl_ensure_code(rhs->nc == n, CPL_ERROR_INCOMPATIBLE_INPUT); |
| 2815 |
|
| 2816 |
aread = self->m; |
| 2817 |
|
| 2818 |
/* bk points to first entry in k'th right hand side */ |
| 2819 |
bk = rhs->m; |
| 2820 |
|
| 2821 |
for (k=0; k < nrhs; k++, bk += n) { |
| 2822 |
|
| 2823 |
/* Forward substitution in column k */ |
| 2824 |
|
| 2825 |
/* Since self and rhs are addressed in the same manner, |
| 2826 |
they can use the same index, j */ |
| 2827 |
ai = aread; /* ai points to first entry in i'th row */ |
| 2828 |
for (i = 0; i < n; i++, ai += n) { |
| 2829 |
sub = 0.0; |
| 2830 |
for (j = 0; j < i; j++) { |
| 2831 |
sub += ai[j] * bk[j]; |
| 2832 |
} |
| 2833 |
cpl_ensure_code(k > 0 || ai[j] != 0.0, CPL_ERROR_DIVISION_BY_ZERO); |
| 2834 |
bk[j] = (bk[j] - sub) / ai[j]; |
| 2835 |
} |
| 2836 |
|
| 2837 |
/* Back substitution in column k */ |
| 2838 |
|
| 2839 |
for (i = n-1; i >= 0; i--) { |
| 2840 |
sub = bk[i]; |
| 2841 |
for (j = i+1; j < n; j++) { |
| 2842 |
sub -= aread[n * j + i] * bk[j]; |
| 2843 |
} |
| 2844 |
bk[i] = sub/aread[n * i + i]; |
| 2845 |
} |
| 2846 |
} |
| 2847 |
|
| 2848 |
return CPL_ERROR_NONE; |
| 2849 |
|
| 2850 |
} |
| 2851 |
|
| 2852 |
|
| 2853 |
/* ---------------------------------------------------------------------------*/ |
| 2854 |
/** |
| 2855 |
* @brief generic 1d vandermonde matrix |
| 2856 |
* |
| 2857 |
* @param sample sampling positions |
| 2858 |
* @param degree degree of polynomial |
| 2859 |
* @param func function evaluating polynomials from [0, degree] at |
| 2860 |
* sampling point |
| 2861 |
* @param offset offset of func (1 for indexing starting with 1) |
| 2862 |
* @return matrix containing the vandermonde matrix |
| 2863 |
*/ |
| 2864 |
/* ---------------------------------------------------------------------------*/ |
| 2865 |
cpl_matrix * vander1d( |
| 2866 |
const cpl_vector * sample, |
| 2867 |
cpl_size degree, |
| 2868 |
void (*func)(double, double *, int), |
| 2869 |
size_t offset) |
| 2870 |
{ |
| 2871 |
size_t i; |
| 2872 |
const size_t nr = cpl_vector_get_size(sample); |
| 2873 |
const size_t nc = degree + 1; |
| 2874 |
cpl_matrix * V = cpl_matrix_new(nr, nc); |
| 2875 |
double * v = cpl_matrix_get_data(V); |
| 2876 |
const double * d = sample->d; |
| 2877 |
for (i = 0; i < nr; i++) { |
| 2878 |
if (offset != 0) { |
| 2879 |
double tmp[nc + offset]; |
| 2880 |
func(d[i], tmp, nc); |
| 2881 |
memcpy(&v[i * nc], &tmp[offset], nc * sizeof(*v)); |
| 2882 |
} |
| 2883 |
else { |
| 2884 |
func(d[i], &v[i * nc], nc); |
| 2885 |
} |
| 2886 |
} |
| 2887 |
|
| 2888 |
return V; |
| 2889 |
} |
| 2890 |
|
| 2891 |
cpl_matrix * vander2d( |
| 2892 |
const cpl_vector * sample_x, |
| 2893 |
const cpl_vector * sample_y, |
| 2894 |
cpl_size degree, |
| 2895 |
void (*func)(double, double, double *, int), |
| 2896 |
size_t offset) |
| 2897 |
{ |
| 2898 |
size_t i; |
| 2899 |
const size_t nr = cpl_vector_get_size(sample_x); |
| 2900 |
const size_t nc = degree + 1; |
| 2901 |
cpl_matrix * V = cpl_matrix_new(nr, nc); |
| 2902 |
double * v = cpl_matrix_get_data(V); |
| 2903 |
const double * dx = sample_x->d; |
| 2904 |
const double * dy = sample_y->d; |
| 2905 |
assert(cpl_vector_get_size(sample_y) == nr); |
| 2906 |
for (i = 0; i < nr; i++) { |
| 2907 |
if (offset != 0) { |
| 2908 |
double tmp[nc + offset]; |
| 2909 |
func(dx[i], dy[i], tmp, nc); |
| 2910 |
memcpy(&v[i * nc], &tmp[offset], nc * sizeof(*v)); |
| 2911 |
} |
| 2912 |
else { |
| 2913 |
func(dx[i], dy[i], &v[i * nc], nc); |
| 2914 |
} |
| 2915 |
} |
| 2916 |
|
| 2917 |
return V; |
| 2918 |
} |
| 2919 |
|
| 2920 |
static void polynomial(double x, double * p, int ncoefs) |
| 2921 |
{ |
| 2922 |
int i; |
| 2923 |
p[0] = 1.; |
| 2924 |
for (i = 1; i < ncoefs; i++) { |
| 2925 |
p[i] = pow(x, i); |
| 2926 |
} |
| 2927 |
} |
| 2928 |
|
| 2929 |
cpl_matrix * polyvander1d( |
| 2930 |
const cpl_vector * sample, |
| 2931 |
cpl_size degree) |
| 2932 |
{ |
| 2933 |
return vander1d(sample, degree, &polynomial, 0); |
| 2934 |
} |
| 2935 |
|
| 2936 |
void lsqfit( |
| 2937 |
const cpl_matrix * design, |
| 2938 |
const cpl_vector * values, |
| 2939 |
const cpl_vector * errors, |
| 2940 |
cpl_matrix **coef) |
| 2941 |
{ |
| 2942 |
/* weight response and design */ |
| 2943 |
cpl_vector * vrhs = cpl_vector_duplicate(errors); |
| 2944 |
cpl_vector_power(vrhs, -1); |
| 2945 |
cpl_matrix * wdesign = cpl_matrix_duplicate(design); |
| 2946 |
long i, j; |
| 2947 |
for (i = 0; i < cpl_vector_get_size(errors); i++) { |
| 2948 |
double w = vrhs->d[i]; |
| 2949 |
for (j = 0; j < cpl_matrix_get_ncol(wdesign); j++) { |
| 2950 |
cpl_matrix_set(wdesign, i, j, |
| 2951 |
cpl_matrix_get(wdesign, i, j) * w); |
| 2952 |
} |
| 2953 |
} |
| 2954 |
|
| 2955 |
cpl_vector_multiply(vrhs, values); |
| 2956 |
cpl_matrix * rhs = cpl_matrix_wrap(cpl_vector_get_size(vrhs), 1, |
| 2957 |
vrhs->d); |
| 2958 |
|
| 2959 |
/* solve Ax = b */ |
| 2960 |
/* cpl_matrix_solve_normal(design, rhs) + covariance */ |
| 2961 |
{ |
| 2962 |
cpl_matrix * At = cpl_matrix_transpose_create(wdesign); |
| 2963 |
cpl_matrix * AtA = cpl_matrix_product_normal_create(At); |
| 2964 |
|
| 2965 |
/* RRt = AtA */ |
| 2966 |
cpl_matrix_decomp_chol(AtA); |
| 2967 |
/* solve for pseudo inverse: (RRt)P=At*/ |
| 2968 |
cpl_matrix_solve_chol(AtA, At); |
| 2969 |
/* compute solution to system Ax=b -> x=Pb */ |
| 2970 |
*coef = cpl_matrix_product_create(At, rhs); |
| 2971 |
/* compute covariance matrix cov(b) = PPt */ |
| 2972 |
/* cpl_matrix * cov = cpl_matrix_new(cpl_matrix_get_ncol(At), */ |
| 2973 |
/* cpl_matrix_get_ncol(At)); */ |
| 2974 |
/* cpl_matrix_product_transpose(cov, At, At); */ |
| 2975 |
|
| 2976 |
cpl_matrix_delete(At); |
| 2977 |
cpl_matrix_delete(AtA); |
| 2978 |
} |
| 2979 |
|
| 2980 |
cpl_matrix_unwrap(rhs); |
| 2981 |
cpl_vector_delete(vrhs); |
| 2982 |
cpl_matrix_delete(wdesign); |
| 2983 |
|
| 2984 |
return; |
| 2985 |
} |
| 2986 |
|
| 2987 |
void lsqfit_nr( |
| 2988 |
double x[], double y[], double sig[], int ndat, double a[], |
| 2989 |
int ma, |
| 2990 |
void (*funcs)(double, double [], int)) |
| 2991 |
{ |
| 2992 |
int i; |
| 2993 |
cpl_vector * vX = cpl_vector_wrap(ndat, &x[1]); |
| 2994 |
cpl_vector * vY = cpl_vector_wrap(ndat, &y[1]); |
| 2995 |
cpl_vector * vS; |
| 2996 |
cpl_matrix * design = vander1d(vX, ma - 1, funcs, 1); |
| 2997 |
cpl_matrix * mcoef; |
| 2998 |
|
| 2999 |
if (sig) { |
| 3000 |
vS = cpl_vector_wrap(ndat, &sig[1]); |
| 3001 |
} |
| 3002 |
else { |
| 3003 |
vS = cpl_vector_new(ndat); |
| 3004 |
for (i= 0; i < ndat; i++) |
| 3005 |
cpl_vector_set(vS, i, 1.); |
| 3006 |
} |
| 3007 |
lsqfit(design, vY, vS, &mcoef); |
| 3008 |
|
| 3009 |
for (i = 1; i <= ma; i++) { |
| 3010 |
a[i] = cpl_matrix_get(mcoef, i - 1, 0); |
| 3011 |
} |
| 3012 |
cpl_vector_unwrap(vX); |
| 3013 |
cpl_vector_unwrap(vY); |
| 3014 |
if (sig) { |
| 3015 |
cpl_vector_unwrap(vS); |
| 3016 |
} |
| 3017 |
else { |
| 3018 |
cpl_vector_delete(vS); |
| 3019 |
} |
| 3020 |
cpl_matrix_delete(design); |
| 3021 |
cpl_matrix_delete(mcoef); |
| 3022 |
} |
| 3023 |
|
| 3024 |
|
| 3025 |
void lsqfit2d_nr( |
| 3026 |
double x[], double y[], double z[], double sig[], int ndat, double a[], |
| 3027 |
int ma, |
| 3028 |
void (*funcs)(double, double, double [], int)) |
| 3029 |
{ |
| 3030 |
int i; |
| 3031 |
cpl_vector * vX = cpl_vector_wrap(ndat, &x[1]); |
| 3032 |
cpl_vector * vY = cpl_vector_wrap(ndat, &y[1]); |
| 3033 |
cpl_vector * vZ = cpl_vector_wrap(ndat, &z[1]); |
| 3034 |
cpl_vector * vS; |
| 3035 |
cpl_matrix * design = vander2d(vX, vY, ma - 1, funcs, 1); |
| 3036 |
cpl_matrix * mcoef; |
| 3037 |
|
| 3038 |
if (sig) { |
| 3039 |
vS = cpl_vector_wrap(ndat, &sig[1]); |
| 3040 |
} |
| 3041 |
else { |
| 3042 |
vS = cpl_vector_new(ndat); |
| 3043 |
for (i= 0; i < ndat; i++) |
| 3044 |
cpl_vector_set(vS, i, 1.); |
| 3045 |
} |
| 3046 |
lsqfit(design, vZ, vS, &mcoef); |
| 3047 |
|
| 3048 |
for (i = 1; i <= ma; i++) { |
| 3049 |
a[i] = cpl_matrix_get(mcoef, i - 1, 0); |
| 3050 |
} |
| 3051 |
cpl_vector_unwrap(vX); |
| 3052 |
cpl_vector_unwrap(vY); |
| 3053 |
cpl_vector_unwrap(vZ); |
| 3054 |
if (sig) { |
| 3055 |
cpl_vector_unwrap(vS); |
| 3056 |
} |
| 3057 |
else { |
| 3058 |
cpl_vector_delete(vS); |
| 3059 |
} |
| 3060 |
cpl_matrix_delete(design); |
| 3061 |
cpl_matrix_delete(mcoef); |
| 3062 |
} |
| 3063 |
|
| 3064 |
|
| 3065 |
cpl_vector * eval_poly( |
| 3066 |
cpl_matrix * design, |
| 3067 |
cpl_matrix * coef) |
| 3068 |
{ |
| 3069 |
cpl_matrix * fvalues = cpl_matrix_product_create(design, coef); |
| 3070 |
cpl_vector * res = cpl_vector_wrap(cpl_matrix_get_nrow(fvalues), |
| 3071 |
cpl_matrix_get_data(fvalues)); |
| 3072 |
cpl_matrix_unwrap(fvalues); |
| 3073 |
return res; |
| 3074 |
} |
| 3075 |
|
| 3076 |
|