implemented a very simple (and presumably slow) complete pivoting pro…

…cedure for rectangular matrices, to be used for the new dynamic state selection

git-svn-id: https://openmodelica.org/svn/OpenModelica/trunk@14427 f25d12d1-65f4-0310-ae8a-bbce733d8d8e

SimulationRuntime/c/math-support/CMakeLists.txt

@@ -5,7 +5,7 @@
 SET(math_support_sources  bigden.c biglag.c dgesv_aux.c dogleg.c dpmpar.c 
                           daux.c ddassl.c ddasrt.c dlamch.c dlinpk.c
                           enorm.c fdjac1.c hybrd.c hybrd1.c hybrj.c lsame.c 
-			  nelmead.c newuoa.c newuob.c  qform.c  qrfac.c r1mpyq.c 
+			  nelmead.c newuoa.c newuob.c  pivot.c qform.c  qrfac.c r1mpyq.c 
                           r1updt.c trsapp.c update.c)
 
 SET(math_support_headers blaswrap.h matrix.h) 
@@ -20,3 +20,5 @@ INSTALL(TARGETS math-support
 
 #INSTALL(FILES ${math_support_headers} DESTINATION include)
 
+# add tests
+ADD_SUBDIRECTORY(test)

SimulationRuntime/c/math-support/pivot.c

@@ -0,0 +1,133 @@
+/* pivot.c
+ provides a simple function to do a full pivot search while doing an LU factorization
+*/
+#include <math.h>
+#include <assert.h>
+//#include "matrix.h"
+
+/* Matrixes using column major order (as in Fortran) */
+#ifndef set_matrix_elt
+#define set_matrix_elt(A,r,c,n_rows,value) A[r + n_rows * c] = value
+#endif
+
+#ifndef get_matrix_elt
+#define get_matrix_elt(A,r,c,n_rows) A[r + n_rows * c]
+#endif
+
+
+/* Matrixes using column major order (as in Fortran) */
+/* colInd, rowInd, n_rows is added implicitly, makes code easier to read but may be considered bad programming style! */
+#define set_pivot_matrix_elt(A,r,c,value) set_matrix_elt(A,rowInd[r],colInd[c],n_rows,value)
+#define get_pivot_matrix_elt(A,r,c) get_matrix_elt(A,rowInd[r],colInd[c],n_rows)
+#define swap(a,b) { int _swap=a; a=b; b=_swap; }
+
+#ifndef min
+#define min(a,b) ((a > b) ? (b) : (a))
+#endif
+
+/* 
+find the maximum element below (and including) line/row start
+*/
+void maxsearch( double *A, int start, int n_rows, int n_cols, int *rowInd, int *colInd, int *maxrow, int *maxcol, double *maxabsval)
+{
+	// temporary variables
+	int row;
+	int col;
+
+	// Initialization
+	int mrow = -1;
+	int mcol = -1;
+	double mabsval = 0.0;
+
+	// go through all rows and columns
+	for(row=start; row<n_rows; row++)
+	{
+		for(col=start; col<n_cols; col++)
+		{
+			double tmp = fabs(get_pivot_matrix_elt(A,row,col));
+			// Compare element to current maximum
+			if (tmp > mabsval)
+			{
+				mrow = row;
+				mcol = col;
+				mabsval = tmp;
+			}
+		}
+	}
+
+	// assert that the matrix is not identical to zero
+	assert(mrow >= 0);
+	assert(mcol >= 0);
+
+	// return result
+	*maxrow = mrow;
+	*maxcol = mcol;
+	*maxabsval = mabsval;
+}
+
+
+
+/*
+pivot performs a full pivotization of a rectangular matrix A of dimension n_cols x n_rows
+rowInd and colInd are vectors of length nrwos and n_cols respectively.
+They hold the old (and new) pivoting information, such that
+	A_pivoted[i,j] = A[rowInd[i], colInd[j]]
+*/
+void pivot( double *A, int n_rows, int n_cols, int *rowInd, int *colInd )
+{
+	// parameter, determines how much larger an element should be before rows and columns are interchanged
+	const double fac = 2.0;
+
+	// temporary variables
+	int row;
+	int i,j;
+	int maxrow;
+	int maxcol;
+	double maxabsval;
+	double pivot;
+
+	// go over all pivot elements
+	for(row=0; row<min(n_rows,n_cols); row++)
+	{
+		// get current pivot
+		pivot = fabs(get_pivot_matrix_elt(A,row,row));
+
+		// find the maximum element in matrix
+		// result is stored in maxrow, maxcol and maxabsval
+		maxsearch(A, row, n_rows, n_cols, rowInd, colInd, &maxrow, &maxcol, &maxabsval);
+
+		// compare max element and pivot (scaled by fac)
+		if (maxabsval > (fac*pivot))
+		{
+			// row interchange
+			swap(rowInd[row], rowInd[maxrow]);
+			// column interchange
+			swap(colInd[row], colInd[maxcol]);
+		}
+
+		// get pivot (without abs, may have changed because of row/column interchange
+		pivot = get_pivot_matrix_elt(A,row,row);
+		assert(pivot != 0);
+
+		// perform one step of Gaussian Elimination
+		for(i=row+1;i<n_rows;i++)
+		{
+			double leader = get_pivot_matrix_elt(A,i,row);
+			if (leader != 0.0)
+			{
+				double scale = -leader/pivot;
+				// set leader to zero
+				set_pivot_matrix_elt(A,i,row, 0.0);
+				// subtract scaled equation from pivot row from current row
+				for(j=row+1;j<n_cols;j++)
+				{
+					double t1 = get_pivot_matrix_elt(A,i,j);
+					double t2 = get_pivot_matrix_elt(A,row,j);
+					double tmp = t1 + scale*t2;
+					set_pivot_matrix_elt(A,i,j, tmp);
+				}
+			}
+		}
+	}
+}
+

SimulationRuntime/c/math-support/test/CMakeLists.txt

@@ -0,0 +1,12 @@
+# Christian Schubert, Christian.Schubert@tu-dresden.de, 2012-12-17
+# CMakefile for compilation of OMC 
+
+# include CTest gives more options (such as running valgrind automatically)
+include(CTest)
+
+#Define values in the "config.h"
+set(CTEST_RETURN_SUCCESS 0) 
+set(CTEST_RETURN_FAIL 1)
+
+ADD_EXECUTABLE (test_pivot ${CMAKE_CURRENT_SOURCE_DIR}/test_pivot.c )
+ADD_TEST(test_simulationruntime_mathsupport_pivot test_pivot)

SimulationRuntime/c/math-support/test/test_pivot.c

@@ -0,0 +1,129 @@
+#include <math.h>
+//#include "matrix.h"
+
+/* Matrixes using column major order (as in Fortran) */
+#ifndef set_matrix_elt
+#define set_matrix_elt(A,r,c,n_rows,value) A[r + n_rows * c] = value
+#endif
+
+#ifndef get_matrix_elt
+#define get_matrix_elt(A,r,c,n_rows) A[r + n_rows * c]
+#endif
+
+// forward declarations
+int test_Pendulum(double x, double y);
+int test_Quaternion(double q0, double q1, double q2, double q3);
+int test_35();
+void pivot( double *A, int n_rows, int n_cols, int *rowInd, int *colInd );
+
+// main 
+int main()
+{
+	// return code
+	int rc;
+
+	// Pendulum
+	if ( (rc = test_Pendulum(1,0)) != 0) return 1000+rc;
+	if ( (rc = test_Pendulum(sqrt(2.0)/2.0,sqrt(2.0)/2.0)) != 0) return 1100+rc;
+	if ( (rc = test_Pendulum(0,1)) != 0) return 1200+rc;
+
+	// Quaternion
+	if ( (rc = test_Quaternion(1,0,0,0)) != 0) return 2000+rc;
+	if ( (rc = test_Quaternion(0,1,0,0)) != 0) return 2100+rc;
+	if ( (rc = test_Quaternion(0,0,1,0)) != 0) return 2200+rc;
+	if ( (rc = test_Quaternion(0,0,0,1)) != 0) return 2300+rc;
+	if ( (rc = test_Quaternion(0.1,0.2,0.3,sqrt(1-0.1*0.1-0.2*0.2-0.3*0.3))) != 0) return 2400+rc;
+
+	// Random Test
+	if ( (rc = test_35()) != 0) return 3000+rc;
+
+	// everything OK
+	return 0;
+}
+
+int test_Pendulum(double x, double y)
+{
+	// row and column indices
+	const int n_rows = 1;
+	const int n_cols = 2;
+	int rowInd[] = {0};
+	int colInd[] = {0,1};
+
+	// Matrix
+	double A[] = {2*x, 2*y};
+
+	// Pivotisierung
+	pivot(A, n_rows, n_cols, rowInd, colInd);
+
+	// test result
+	if (rowInd[0] != 0) return 1;
+	if (colInd[0] != (x >= y) ? 0 : 1 ) return 10;
+	if (colInd[1] != (x >= y) ? 1 : 0 ) return 11;
+
+	// everything is fine
+	return 0;
+}
+
+int test_Quaternion(double q0, double q1, double q2, double q3)
+{
+	// row and column indices
+	const int n_rows = 1;
+	const int n_cols = 4;
+	int rowInd[] = {0};
+	int colInd[] = {0,1,2,3};
+
+	// Matrix
+	double A[] = {2*q0, 2*q1, 2*q2, 2*q3};
+
+	// Pivotisierung
+	pivot(A, n_rows, n_cols, rowInd, colInd);
+
+	// test result
+	if (rowInd[0] != 0) return 1;
+	if ((colInd[0] < 0) || (colInd[0] > 3)) return 10;
+	if (colInd[0] == 0) 
+		if ((q0 < q1) || (q0 < q2) || (q0 < q3)) return 11;
+	if (colInd[0] == 1) 
+		if ((q1 < q0) || (q1 < q2) || (q1 < q3)) return 12;
+	if (colInd[0] == 2) 
+		if ((q2 < q0) || (q2 < q1) || (q2 < q3)) return 13;
+	if (colInd[0] == 3) 
+		if ((q3 < q0) || (q3 < q1) || (q3 < q2)) return 14;
+
+	// everything is fine
+	return 0;
+}
+
+// a system that has been generated by random
+int test_35()
+{
+	// row and column indices
+	const int n_rows=3;
+	const int n_cols=5;
+	int rowInd[] = {0,1,2};
+	int colInd[] = {0,1,2,3,4};
+
+	// Matrix (transposed, since columns come first)
+	double A[] = { 14.1886, 42.1761, 91.5736,
+                   79.2207, 95.9492, 65.5741,
+                    3.5712, 84.9129, 93.3993,
+                   67.8735, 75.7740, 74.3132,
+				   39.2227, 65.5478, 17.1187 };
+
+
+	// Pivotisierung
+	pivot(A, n_rows, n_cols, rowInd, colInd);
+
+	// test result
+	if (rowInd[0] != 1) return 1;
+	if (rowInd[1] != 0) return 2;
+	if (rowInd[2] != 2) return 3;
+	if (colInd[0] != 1) return 11;
+	if (colInd[1] != 2) return 11;
+	if (colInd[2] != 0) return 11;
+	if (colInd[3] != 3) return 11;
+	if (colInd[4] != 4) return 11;
+
+	// everything is fine
+	return 0;
+}