OMCompiler/SimulationRuntime/c/math-support/pivot.c
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /* | ||
| 2 | * This file belongs to the OpenModelica Run-Time System | ||
| 3 | * | ||
| 4 | * Copyright (c) 1998-2026, Open Source Modelica Consortium (OSMC), c/o Linköpings | ||
| 5 | * universitet, Department of Computer and Information Science, SE-58183 Linköping, Sweden. All rights | ||
| 6 | * reserved. | ||
| 7 | * | ||
| 8 | * THIS PROGRAM IS PROVIDED UNDER THE TERMS OF THE BSD NEW LICENSE OR THE | ||
| 9 | * AGPL VERSION 3 LICENSE OR THE OSMC PUBLIC LICENSE (OSMC-PL) VERSION 1.8. ANY | ||
| 10 | * USE, REPRODUCTION OR DISTRIBUTION OF THIS PROGRAM CONSTITUTES RECIPIENT'S | ||
| 11 | * ACCEPTANCE OF THE BSD NEW LICENSE OR THE OSMC PUBLIC LICENSE OR THE AGPL | ||
| 12 | * VERSION 3, ACCORDING TO RECIPIENTS CHOICE. | ||
| 13 | * | ||
| 14 | * The OpenModelica software and the OSMC (Open Source Modelica Consortium) Public License | ||
| 15 | * (OSMC-PL) are obtained from OSMC, either from the above address, from the URLs: | ||
| 16 | * http://www.openmodelica.org or https://github.com/OpenModelica/ or | ||
| 17 | * http://www.ida.liu.se/projects/OpenModelica, and in the OpenModelica distribution. GNU | ||
| 18 | * AGPL version 3 is obtained from: https://www.gnu.org/licenses/licenses.html#GPL. The BSD NEW | ||
| 19 | * License is obtained from: http://www.opensource.org/licenses/BSD-3-Clause. | ||
| 20 | * | ||
| 21 | * This program is distributed WITHOUT ANY WARRANTY; without even the implied warranty of | ||
| 22 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, EXCEPT AS EXPRESSLY | ||
| 23 | * SET FORTH IN THE BY RECIPIENT SELECTED SUBSIDIARY LICENSE CONDITIONS OF | ||
| 24 | * OSMC-PL. | ||
| 25 | * | ||
| 26 | */ | ||
| 27 | |||
| 28 | /* | ||
| 29 | * provides a simple function to do a full pivot search while doing an LU factorization | ||
| 30 | */ | ||
| 31 | |||
| 32 | #include <math.h> | ||
| 33 | #include <assert.h> | ||
| 34 | #include "../openmodelica.h" | ||
| 35 | |||
| 36 | /* Matrixes using column major order (as in Fortran) */ | ||
| 37 | #ifndef set_matrix_elt | ||
| 38 | #define set_matrix_elt(A,r,c,n_rows,value) A[r + n_rows * c] = value | ||
| 39 | #endif | ||
| 40 | |||
| 41 | #ifndef get_matrix_elt | ||
| 42 | #define get_matrix_elt(A,r,c,n_rows) A[r + n_rows * c] | ||
| 43 | #endif | ||
| 44 | |||
| 45 | |||
| 46 | /* Matrixes using column major order (as in Fortran) */ | ||
| 47 | /* colInd, rowInd, n_rows is added implicitly, makes code easier to read but may be considered bad programming style! */ | ||
| 48 | #define set_pivot_matrix_elt(A,r,c,value) set_matrix_elt(A,rowInd[r],colInd[c],n_rows,value) | ||
| 49 | /* #define set_pivot_matrix_elt(A,r,c,value) set_matrix_elt(A,colInd[c],rowInd[r],n_cols,value) */ | ||
| 50 | #define get_pivot_matrix_elt(A,r,c) get_matrix_elt(A,rowInd[r],colInd[c],n_rows) | ||
| 51 | /* #define get_pivot_matrix_elt(A,r,c) get_matrix_elt(A,colInd[c],rowInd[r],n_cols) */ | ||
| 52 | #define swap(a,b) { modelica_integer _swap=a; a=b; b=_swap; } | ||
| 53 | |||
| 54 | #ifndef min | ||
| 55 | #define min(a,b) ((a > b) ? (b) : (a)) | ||
| 56 | #endif | ||
| 57 | |||
| 58 | /* | ||
| 59 | find the maximum element below (and including) line/row start | ||
| 60 | */ | ||
| 61 | ✗ | int maxsearch( double *A, modelica_integer start, modelica_integer n_rows, modelica_integer n_cols, modelica_integer *rowInd, modelica_integer *colInd, modelica_integer *maxrow, modelica_integer *maxcol, double *maxabsval) | |
| 62 | { | ||
| 63 | /* temporary variables */ | ||
| 64 | modelica_integer row; | ||
| 65 | modelica_integer col; | ||
| 66 | |||
| 67 | /* Initialization */ | ||
| 68 | modelica_integer mrow = -1; | ||
| 69 | modelica_integer mcol = -1; | ||
| 70 | double mabsval = 0.0; | ||
| 71 | |||
| 72 | /* go through all rows and columns */ | ||
| 73 | ✗ | for(row=start; row<n_rows; row++) | |
| 74 | { | ||
| 75 | ✗ | for(col=start; col<n_cols; col++) | |
| 76 | { | ||
| 77 | ✗ | double tmp = fabs(get_pivot_matrix_elt(A,row,col)); | |
| 78 | /* Compare element to current maximum */ | ||
| 79 | ✗ | if (tmp > mabsval) | |
| 80 | { | ||
| 81 | mrow = row; | ||
| 82 | mcol = col; | ||
| 83 | mabsval = tmp; | ||
| 84 | } | ||
| 85 | } | ||
| 86 | } | ||
| 87 | |||
| 88 | /* assert that the matrix is not identical to zero */ | ||
| 89 | ✗ | if ((mrow < 0) || (mcol < 0)) return -1; | |
| 90 | |||
| 91 | /* return result */ | ||
| 92 | ✗ | *maxrow = mrow; | |
| 93 | ✗ | *maxcol = mcol; | |
| 94 | ✗ | *maxabsval = mabsval; | |
| 95 | ✗ | return 0; | |
| 96 | } | ||
| 97 | |||
| 98 | |||
| 99 | |||
| 100 | /* | ||
| 101 | pivot performs a full pivotization of a rectangular matrix A of dimension n_cols x n_rows | ||
| 102 | rowInd and colInd are vectors of length nrwos and n_cols respectively. | ||
| 103 | They hold the old (and new) pivoting information, such that | ||
| 104 | A_pivoted[i,j] = A[rowInd[i], colInd[j]] | ||
| 105 | */ | ||
| 106 | ✗ | int pivot( double *A, modelica_integer n_rows, modelica_integer n_cols, modelica_integer *rowInd, modelica_integer *colInd ) | |
| 107 | { | ||
| 108 | /* parameter, determines how much larger an element should be before rows and columns are interchanged */ | ||
| 109 | const double fac = 1.125; /* approved by dymola ;) */ | ||
| 110 | |||
| 111 | /* temporary variables */ | ||
| 112 | modelica_integer row; | ||
| 113 | modelica_integer i,j; | ||
| 114 | modelica_integer maxrow; | ||
| 115 | modelica_integer maxcol; | ||
| 116 | double maxabsval; | ||
| 117 | double pivot; | ||
| 118 | |||
| 119 | /* go over all pivot elements */ | ||
| 120 | ✗ | for(row=0; row<min(n_rows,n_cols); row++) | |
| 121 | { | ||
| 122 | /* get current pivot */ | ||
| 123 | ✗ | pivot = fabs(get_pivot_matrix_elt(A,row,row)); | |
| 124 | |||
| 125 | /* find the maximum element in matrix | ||
| 126 | result is stored in maxrow, maxcol and maxabsval */ | ||
| 127 | ✗ | if (maxsearch(A, row, n_rows, n_cols, rowInd, colInd, &maxrow, &maxcol, &maxabsval) != 0) return -1; | |
| 128 | |||
| 129 | |||
| 130 | /* compare max element and pivot (scaled by fac) */ | ||
| 131 | ✗ | if (maxabsval > (fac*pivot)) | |
| 132 | { | ||
| 133 | /* row interchange */ | ||
| 134 | ✗ | swap(rowInd[row], rowInd[maxrow]); | |
| 135 | /* column interchange */ | ||
| 136 | ✗ | swap(colInd[row], colInd[maxcol]); | |
| 137 | } | ||
| 138 | |||
| 139 | /* get pivot (without abs, may have changed because of row/column interchange */ | ||
| 140 | ✗ | pivot = get_pivot_matrix_elt(A,row,row); | |
| 141 | /* internal error, pivot element should never be zero if maxsearch succeeded */ | ||
| 142 | ✗ | assert(pivot != 0); | |
| 143 | |||
| 144 | /* perform one step of Gaussian Elimination */ | ||
| 145 | ✗ | for(i=row+1;i<n_rows;i++) | |
| 146 | { | ||
| 147 | ✗ | double leader = get_pivot_matrix_elt(A,i,row); | |
| 148 | ✗ | if (leader != 0.0) | |
| 149 | { | ||
| 150 | ✗ | double scale = -leader/pivot; | |
| 151 | /* set leader to zero */ | ||
| 152 | ✗ | set_pivot_matrix_elt(A,i,row, 0.0); | |
| 153 | /* subtract scaled equation from pivot row from current row */ | ||
| 154 | ✗ | for(j=row+1;j<n_cols;j++) | |
| 155 | { | ||
| 156 | ✗ | double t1 = get_pivot_matrix_elt(A,i,j); | |
| 157 | ✗ | double t2 = get_pivot_matrix_elt(A,row,j); | |
| 158 | ✗ | double tmp = t1 + scale*t2; | |
| 159 | ✗ | set_pivot_matrix_elt(A,i,j, tmp); | |
| 160 | } | ||
| 161 | } | ||
| 162 | } | ||
| 163 | } | ||
| 164 | /* all fine */ | ||
| 165 | return 0; | ||
| 166 | } | ||
| 167 |