OMCompiler/SimulationRuntime/cpp/Core/Math/Functions.cpp
| 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 | /** @addtogroup math | ||
| 29 | * @{ | ||
| 30 | */ | ||
| 31 | |||
| 32 | #include <Core/ModelicaDefine.h> | ||
| 33 | #include <Core/Modelica.h> | ||
| 34 | #include <Core/Math/Functions.h> | ||
| 35 | #include <stdexcept> | ||
| 36 | |||
| 37 | //#include <Core/Utils/extension/logger.hpp> | ||
| 38 | |||
| 39 | /* Matrixes using column major order (as in Fortran) */ | ||
| 40 | #ifndef set_matrix_elt | ||
| 41 | #define set_matrix_elt(A,r,c,n_rows,value) A[r + n_rows * c] = value | ||
| 42 | #endif | ||
| 43 | |||
| 44 | #ifndef get_matrix_elt | ||
| 45 | #define get_matrix_elt(A,r,c,n_rows) A[r + n_rows * c] | ||
| 46 | #endif | ||
| 47 | |||
| 48 | /* Matrixes using column major order (as in Fortran) */ | ||
| 49 | /* colInd, rowInd, n_rows is added implicitly, makes code easier to read but may be considered bad programming style! */ | ||
| 50 | #define set_pivot_matrix_elt(A,r,c,value) set_matrix_elt(A,rowInd[r],colInd[c],n_rows,value) | ||
| 51 | /* #define set_pivot_matrix_elt(A,r,c,value) set_matrix_elt(A,colInd[c],rowInd[r],n_cols,value) */ | ||
| 52 | #define get_pivot_matrix_elt(A,r,c) get_matrix_elt(A,rowInd[r],colInd[c],n_rows) | ||
| 53 | /* #define get_pivot_matrix_elt(A,r,c) get_matrix_elt(A,colInd[c],rowInd[r],n_cols) */ | ||
| 54 | #define swap(a,b) { int _swap=a; a=b; b=_swap; } | ||
| 55 | |||
| 56 | 179313902 | double division (const double &a,const double &b, bool throwEx,const char* text) | |
| 57 | { | ||
| 58 |
2/2✓ Branch 0 taken 179313900 times.
✓ Branch 1 taken 2 times.
|
179313902 | if(b != 0) |
| 59 | 179313900 | return a/b ; | |
| 60 | else | ||
| 61 | { | ||
| 62 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
|
2 | if(a==0) |
| 63 | { | ||
| 64 | //LOGGER_WRITE("Division by Zero: Solver will try to handle division by zero with minimu norm for" + string(text), LC_INIT, LL_DEBUG); | ||
| 65 | return 0; | ||
| 66 | } | ||
| 67 | |||
| 68 | ✗ | if(throwEx) | |
| 69 | ✗ | throw ModelicaSimulationError(UTILITY,"Division by zero: "+string(text)); | |
| 70 | else | ||
| 71 | { | ||
| 72 | //LOGGER_WRITE("Division: Solver will try to handle division by zero for" + string(text), LC_INIT, LL_DEBUG); | ||
| 73 | return a; | ||
| 74 | } | ||
| 75 | } | ||
| 76 | } | ||
| 77 | |||
| 78 | /* | ||
| 79 | find the maximum element below (and including) line/row start | ||
| 80 | */ | ||
| 81 | 581 | int maxsearch(double *A, int start, int n_rows, int n_cols, int *rowInd, int *colInd, int *maxrow, int *maxcol, double *maxabsval) | |
| 82 | { | ||
| 83 | /* temporary variables */ | ||
| 84 | int row; | ||
| 85 | int col; | ||
| 86 | |||
| 87 | /* Initialization */ | ||
| 88 | int mrow = -1; | ||
| 89 | int mcol = -1; | ||
| 90 | double mabsval = 0.0; | ||
| 91 | |||
| 92 | /* go through all rows and columns */ | ||
| 93 |
2/2✓ Branch 0 taken 581 times.
✓ Branch 1 taken 581 times.
|
1162 | for(row=start; row<n_rows; row++) |
| 94 | { | ||
| 95 |
2/2✓ Branch 0 taken 2324 times.
✓ Branch 1 taken 581 times.
|
2905 | for(col=start; col<n_cols; col++) |
| 96 | { | ||
| 97 | 2324 | double tmp = fabs(get_pivot_matrix_elt(A,row,col)); | |
| 98 | /* Compare element to current maximum */ | ||
| 99 |
2/2✓ Branch 0 taken 581 times.
✓ Branch 1 taken 1743 times.
|
2324 | if (tmp > mabsval) |
| 100 | { | ||
| 101 | mrow = row; | ||
| 102 | mcol = col; | ||
| 103 | mabsval = tmp; | ||
| 104 | } | ||
| 105 | } | ||
| 106 | } | ||
| 107 | |||
| 108 | /* assert that the matrix is not identical to zero */ | ||
| 109 |
1/2✓ Branch 0 taken 581 times.
✗ Branch 1 not taken.
|
581 | if ((mrow < 0) || (mcol < 0)) return -1; |
| 110 | |||
| 111 | /* return result */ | ||
| 112 | 581 | *maxrow = mrow; | |
| 113 | 581 | *maxcol = mcol; | |
| 114 | 581 | *maxabsval = mabsval; | |
| 115 | 581 | return 0; | |
| 116 | } | ||
| 117 | |||
| 118 | /* | ||
| 119 | pivot performs a full pivotization of a rectangular matrix A of dimension n_cols x n_rows | ||
| 120 | rowInd and colInd are vectors of length nrwos and n_cols respectively. | ||
| 121 | They hold the old (and new) pivoting information, such that | ||
| 122 | A_pivoted[i,j] = A[rowInd[i], colInd[j]] | ||
| 123 | */ | ||
| 124 | 581 | int pivot(double *A, int n_rows, int n_cols, int *rowInd, int *colInd) | |
| 125 | { | ||
| 126 | /* parameter, determines how much larger an element should be before rows and columns are interchanged */ | ||
| 127 | const double fac = 1.125; /* approved by dymola ;) */ | ||
| 128 | |||
| 129 | /* temporary variables */ | ||
| 130 | int row; | ||
| 131 | int i,j; | ||
| 132 | int maxrow; | ||
| 133 | int maxcol; | ||
| 134 | double maxabsval; | ||
| 135 | double pivot; | ||
| 136 | |||
| 137 | /* go over all pivot elements */ | ||
| 138 |
2/2✓ Branch 0 taken 581 times.
✓ Branch 1 taken 581 times.
|
1743 | for(row=0; row<min(n_rows,n_cols); row++) |
| 139 | { | ||
| 140 | /* get current pivot */ | ||
| 141 | 581 | pivot = fabs(get_pivot_matrix_elt(A,row,row)); | |
| 142 | |||
| 143 | /* find the maximum element in matrix | ||
| 144 | result is stored in maxrow, maxcol and maxabsval */ | ||
| 145 |
1/2✓ Branch 1 taken 581 times.
✗ Branch 2 not taken.
|
581 | if (maxsearch(A, row, n_rows, n_cols, rowInd, colInd, &maxrow, &maxcol, &maxabsval) != 0) return -1; |
| 146 | |||
| 147 | |||
| 148 | /* compare max element and pivot (scaled by fac) */ | ||
| 149 |
2/2✓ Branch 0 taken 1 time.
✓ Branch 1 taken 580 times.
|
581 | if (maxabsval > (fac*pivot)) |
| 150 | { | ||
| 151 | /* row interchange */ | ||
| 152 | 1 | swap(rowInd[row], rowInd[maxrow]); | |
| 153 | /* column interchange */ | ||
| 154 | 1 | swap(colInd[row], colInd[maxcol]); | |
| 155 | } | ||
| 156 | |||
| 157 | /* get pivot (without abs, may have changed because of row/column interchange */ | ||
| 158 | 581 | pivot = get_pivot_matrix_elt(A,row,row); | |
| 159 | /* internal error, pivot element should never be zero if maxsearch succeeded */ | ||
| 160 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 581 times.
|
581 | if(pivot == 0) |
| 161 | ✗ | throw ModelicaSimulationError(UTILITY,"pivot element is zero "); | |
| 162 | |||
| 163 | /* perform one step of Gaussian Elimination */ | ||
| 164 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 581 times.
|
581 | for(i=row+1;i<n_rows;i++) |
| 165 | { | ||
| 166 | ✗ | double leader = get_pivot_matrix_elt(A,i,row); | |
| 167 | ✗ | if (leader != 0.0) | |
| 168 | { | ||
| 169 | ✗ | double scale = -leader/pivot; | |
| 170 | /* set leader to zero */ | ||
| 171 | ✗ | set_pivot_matrix_elt(A,i,row, 0.0); | |
| 172 | /* subtract scaled equation from pivot row from current row */ | ||
| 173 | ✗ | for(j=row+1;j<n_cols;j++) | |
| 174 | { | ||
| 175 | ✗ | double t1 = get_pivot_matrix_elt(A,i,j); | |
| 176 | ✗ | double t2 = get_pivot_matrix_elt(A,row,j); | |
| 177 | ✗ | double tmp = t1 + scale*t2; | |
| 178 | ✗ | set_pivot_matrix_elt(A,i,j, tmp); | |
| 179 | } | ||
| 180 | } | ||
| 181 | } | ||
| 182 | } | ||
| 183 | /* all fine */ | ||
| 184 | return 0; | ||
| 185 | } | ||
| 186 | |||
| 187 | |||
| 188 | /** @} */ // end of math | ||
| 189 |