From 1a0e5a20ae4f4a2d9fc373038966385cfed574ca Mon Sep 17 00:00:00 2001 From: Bertrand Coconnier Date: Mon, 5 Jun 2017 17:18:52 +0200 Subject: [PATCH] Fixed LaRCSim matrix computation routines. There was a couple of typos in the routine. This code will be used by the AI wake feature. Also added a test for the matrix code to make sure they will be no regression. --- src/FDM/LaRCsim/ls_matrix.c | 10 +-- tests/CMakeLists.txt | 4 + tests/test_ls_matrix.cxx | 161 ++++++++++++++++++++++++++++++++++++ 3 files changed, 167 insertions(+), 8 deletions(-) create mode 100644 tests/test_ls_matrix.cxx diff --git a/src/FDM/LaRCsim/ls_matrix.c b/src/FDM/LaRCsim/ls_matrix.c index a85d8e773..a25f0edab 100644 --- a/src/FDM/LaRCsim/ls_matrix.c +++ b/src/FDM/LaRCsim/ls_matrix.c @@ -223,13 +223,7 @@ int nr_gaussj(double **a, int n, double **b, int m) if (irow != icol) { -/* for (l=1;1<=n;l++) SWAP(a[irow][l],a[icol][l]) */ - for (l=1;l<=n;l++) - { - temp=a[irow][l]; - a[irow][l]=a[icol][l]; - a[icol][l]=temp; - } + for (l=1;l<=n;l++) SWAP(a[irow][l],a[icol][l]) if (bexists) for (l=1;l<=m;l++) SWAP(b[irow][l],b[icol][l]) } indxr[i] = irow; /* We are now ready to divide the pivot row */ @@ -245,7 +239,7 @@ int nr_gaussj(double **a, int n, double **b, int m) dum = a[ll][icol]; a[ll][icol] = 0.0; for (l=1;l<=n;l++) a[ll][l] -= a[icol][l]*dum; - if (bexists) for (l=1;l<=m;l++) b[ll][i] -= b[icol][l]*dum; + if (bexists) for (l=1;l<=m;l++) b[ll][l] -= b[icol][l]*dum; } } diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index c60458815..79c90055b 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -109,3 +109,7 @@ endmacro() flightgear_test(test_navs test_navaids2.cxx) flightgear_test(test_flightplan test_flightplan.cxx) + +add_executable(test_ls_matrix test_ls_matrix.cxx ${CMAKE_SOURCE_DIR}/src/FDM/LaRCsim/ls_matrix.c) +target_link_libraries(test_ls_matrix SimGearCore) +add_test(test_ls_matrix ${EXECUTABLE_OUTPUT_PATH}/test_ls_matrix) diff --git a/tests/test_ls_matrix.cxx b/tests/test_ls_matrix.cxx new file mode 100644 index 000000000..31faf78f3 --- /dev/null +++ b/tests/test_ls_matrix.cxx @@ -0,0 +1,161 @@ +#include +#include +extern "C" { +#include "src/FDM/LaRCsim/ls_matrix.h" +} + +void testCopyMatrix() +{ + int nelm = 20; + double **src = nr_matrix(1, nelm, 1, nelm); + double **dest = nr_matrix(1, nelm, 1, nelm); + double invmaxlong = 1.0/(double)RAND_MAX; + + for (int i=1; i<=nelm; ++i) + for (int j=1; j<=nelm; ++j) + src[i][j] = 2.0 - 4.0*invmaxlong*(double) rand(); + + nr_copymat(src, nelm, dest); + + for (int i=1; i<=nelm; ++i) + for (int j=1; j<=nelm; ++j) + SG_CHECK_EQUAL(src[i][j], dest[i][j]); + + nr_free_matrix(src, 1, nelm, 1, nelm); + nr_free_matrix(dest, 1, nelm, 1, nelm); +} + +void testIdentityMatrix() +{ + int nelm = 10; + double **id = nr_matrix(1, nelm, 1, nelm); + + for (int i=1; i<=nelm; ++i) + for (int j=1; j<=nelm; ++j) + id[i][j] = i == j ? 1.0 : 0.0; + + nr_gaussj(id, nelm, 0, 0); + + for (int i=1; i<=nelm; ++i) + for (int j=1; j<=nelm; ++j) + SG_CHECK_EQUAL_EP(id[i][j], i == j ? 1.0 : 0.0); + + nr_free_matrix(id, 1, nelm, 1, nelm); +} + +void testOrthogonalMatrix() +{ + int nelm = 3; + double **m = nr_matrix(1, nelm, 1, nelm); + double **inv = nr_matrix(1, nelm, 1, nelm); + double angle = M_PI/3.0; + + m[1][1] = cos(angle); + m[1][2] = sin(angle); + m[1][3] = 0.0; + m[2][1] = -m[1][2]; + m[2][2] = m[1][1]; + m[2][3] = 0.0; + m[3][1] = 0.0; + m[3][2] = 0.0; + m[3][3] = 1.0; + + nr_copymat(m, nelm, inv); + nr_gaussj(inv, nelm, 0, 0); + + for (int i=1; i<=nelm; ++i) + for (int j=1; j<=nelm; ++j) + SG_CHECK_EQUAL_EP(m[i][j], inv[j][i]); + + nr_free_matrix(m, 1, nelm, 1, nelm); + nr_free_matrix(inv, 1, nelm, 1, nelm); +} + +void testRandomMatrix() +{ + int nelm = 20; + double **src = nr_matrix(1, nelm, 1, nelm); + double **inv = nr_matrix(1, nelm, 1, nelm); + double **id = nr_matrix(1, nelm, 1, nelm); + double invmaxlong = 1.0/(double)RAND_MAX; + + for (int i=1; i<=nelm; ++i) + for (int j=1; j<=nelm; ++j) + src[i][j] = 2.0 - 4.0*invmaxlong*(double) rand(); + + nr_copymat(src, nelm, inv); + nr_gaussj(inv, nelm, 0, 0); + + for (int i=1; i<=nelm; ++i) + for (int j=1; j<=nelm; ++j) { + id[i][j] = 0.0; + for (int k=1; k<=nelm; ++k) + id[i][j] += src[i][k]*inv[k][j]; + } + + for (int i=1; i<=nelm; ++i) + for (int j=1; j<=nelm; ++j) + SG_CHECK_EQUAL_EP(id[i][j], i == j ? 1.0 : 0.0); + + nr_free_matrix(src, 1, nelm, 1, nelm); + nr_free_matrix(inv, 1, nelm, 1, nelm); + nr_free_matrix(id, 1, nelm, 1, nelm); +} + +void testSolveLinearSystem() +{ + int nelm = 20; + double **src = nr_matrix(1, nelm, 1, nelm); + double **inv = nr_matrix(1, nelm, 1, nelm); + double **rhs = nr_matrix(1, nelm, 1, 1); + double **sol = nr_matrix(1, nelm, 1, 1); + double **check = nr_matrix(1, nelm, 1, nelm); + double invmaxlong = 1.0/(double)RAND_MAX; + + for (int i=1; i<=nelm; ++i) + for (int j=1; j<=nelm; ++j) + src[i][j] = 2.0 - 4.0*invmaxlong*(double) rand(); + + for (int i=1; i<=nelm; ++i) { + rhs[i][1] = 2.0+cos(i*M_PI/nelm); + sol[i][1] = rhs[i][1]; + } + + nr_copymat(src, nelm, inv); + nr_gaussj(inv, nelm, sol, 1); + + for (int i=1; i<=nelm; ++i) + for (int j=1; j<=nelm; ++j) { + check[i][j] = 0.0; + for (int k=1; k<=nelm; ++k) + check[i][j] += src[i][k]*inv[k][j]; + } + + for (int i=1; i<=nelm; ++i) + for (int j=1; j<=nelm; ++j) + SG_CHECK_EQUAL_EP(check[i][j], i == j ? 1.0 : 0.0); + + for (int i=1; i<=nelm; ++i) { + check[i][1] = 0.0; + for (int j=1; j<=nelm; ++j) + check[i][1] += src[i][j]*sol[j][1]; + } + + for (int i=1; i<=nelm; ++i) + SG_CHECK_EQUAL_EP(check[i][1], rhs[i][1]); + + nr_free_matrix(src, 1, nelm, 1, nelm); + nr_free_matrix(inv, 1, nelm, 1, nelm); + nr_free_matrix(check, 1, nelm, 1, nelm); + nr_free_matrix(rhs, 1, nelm, 1, 1); + nr_free_matrix(sol, 1, nelm, 1, 1); +} + +int main(int argc, char* argv[]) +{ + testCopyMatrix(); + testIdentityMatrix(); + testOrthogonalMatrix(); + testRandomMatrix(); + testSolveLinearSystem(); +}