diff --git a/native/MagnetoLib/.gitignore b/native/MagnetoLib/.gitignore deleted file mode 100644 index 1ee53850b..000000000 --- a/native/MagnetoLib/.gitignore +++ /dev/null @@ -1,362 +0,0 @@ -## Ignore Visual Studio temporary files, build results, and -## files generated by popular Visual Studio add-ons. -## -## Get latest from https://github.com/github/gitignore/blob/master/VisualStudio.gitignore - -# User-specific files -*.rsuser -*.suo -*.user -*.userosscache -*.sln.docstates - -# User-specific files (MonoDevelop/Xamarin Studio) -*.userprefs - -# Mono auto generated files -mono_crash.* - -# Build results -[Dd]ebug/ -[Dd]ebugPublic/ -[Rr]elease/ -[Rr]eleases/ -x64/ -x86/ -[Ww][Ii][Nn]32/ -[Aa][Rr][Mm]/ -[Aa][Rr][Mm]64/ -bld/ -[Bb]in/ -[Oo]bj/ -[Ll]og/ -[Ll]ogs/ - -# Visual Studio 2015/2017 cache/options directory -.vs/ -# Uncomment if you have tasks that create the project's static files in wwwroot -#wwwroot/ - -# Visual Studio 2017 auto generated files -Generated\ Files/ - -# MSTest test Results -[Tt]est[Rr]esult*/ -[Bb]uild[Ll]og.* - -# NUnit -*.VisualState.xml -TestResult.xml -nunit-*.xml - -# Build Results of an ATL Project -[Dd]ebugPS/ -[Rr]eleasePS/ -dlldata.c - -# Benchmark Results -BenchmarkDotNet.Artifacts/ - -# .NET Core -project.lock.json -project.fragment.lock.json -artifacts/ - -# ASP.NET Scaffolding -ScaffoldingReadMe.txt - -# StyleCop -StyleCopReport.xml - -# Files built by Visual Studio -*_i.c -*_p.c -*_h.h -*.ilk -*.meta -*.obj -*.iobj -*.pch -*.pdb -*.ipdb -*.pgc -*.pgd -*.rsp -*.sbr -*.tlb -*.tli -*.tlh -*.tmp -*.tmp_proj -*_wpftmp.csproj -*.log -*.vspscc -*.vssscc -.builds -*.pidb -*.svclog -*.scc - -# Chutzpah Test files -_Chutzpah* - -# Visual C++ cache files -ipch/ -*.aps -*.ncb -*.opendb -*.opensdf -*.sdf -*.cachefile -*.VC.db -*.VC.VC.opendb - -# Visual Studio profiler -*.psess -*.vsp -*.vspx -*.sap - -# Visual Studio Trace Files -*.e2e - -# TFS 2012 Local Workspace -$tf/ - -# Guidance Automation Toolkit -*.gpState - -# ReSharper is a .NET coding add-in -_ReSharper*/ -*.[Rr]e[Ss]harper -*.DotSettings.user - -# TeamCity is a build add-in -_TeamCity* - -# DotCover is a Code Coverage Tool -*.dotCover - -# AxoCover is a Code Coverage Tool -.axoCover/* -!.axoCover/settings.json - -# Coverlet is a free, cross platform Code Coverage Tool -coverage*.json -coverage*.xml -coverage*.info - -# Visual Studio code coverage results -*.coverage -*.coveragexml - -# NCrunch -_NCrunch_* -.*crunch*.local.xml -nCrunchTemp_* - -# MightyMoose -*.mm.* -AutoTest.Net/ - -# Web workbench (sass) -.sass-cache/ - -# Installshield output folder -[Ee]xpress/ - -# DocProject is a documentation generator add-in -DocProject/buildhelp/ -DocProject/Help/*.HxT -DocProject/Help/*.HxC -DocProject/Help/*.hhc -DocProject/Help/*.hhk -DocProject/Help/*.hhp -DocProject/Help/Html2 -DocProject/Help/html - -# Click-Once directory -publish/ - -# Publish Web Output -*.[Pp]ublish.xml -*.azurePubxml -# Note: Comment the next line if you want to checkin your web deploy settings, -# but database connection strings (with potential passwords) will be unencrypted -*.pubxml -*.publishproj - -# Microsoft Azure Web App publish settings. Comment the next line if you want to -# checkin your Azure Web App publish settings, but sensitive information contained -# in these scripts will be unencrypted -PublishScripts/ - -# NuGet Packages -*.nupkg -# NuGet Symbol Packages -*.snupkg -# The packages folder can be ignored because of Package Restore -**/[Pp]ackages/* -# except build/, which is used as an MSBuild target. -!**/[Pp]ackages/build/ -# Uncomment if necessary however generally it will be regenerated when needed -#!**/[Pp]ackages/repositories.config -# NuGet v3's project.json files produces more ignorable files -*.nuget.props -*.nuget.targets - -# Microsoft Azure Build Output -csx/ -*.build.csdef - -# Microsoft Azure Emulator -ecf/ -rcf/ - -# Windows Store app package directories and files -AppPackages/ -BundleArtifacts/ -Package.StoreAssociation.xml -_pkginfo.txt -*.appx -*.appxbundle -*.appxupload - -# Visual Studio cache files -# files ending in .cache can be ignored -*.[Cc]ache -# but keep track of directories ending in .cache -!?*.[Cc]ache/ - -# Others -ClientBin/ -~$* -*~ -*.dbmdl -*.dbproj.schemaview -*.jfm -*.pfx -*.publishsettings -orleans.codegen.cs - -# Including strong name files can present a security risk -# (https://github.com/github/gitignore/pull/2483#issue-259490424) -#*.snk - -# Since there are multiple workflows, uncomment next line to ignore bower_components -# (https://github.com/github/gitignore/pull/1529#issuecomment-104372622) -#bower_components/ - -# RIA/Silverlight projects -Generated_Code/ - -# Backup & report files from converting an old project file -# to a newer Visual Studio version. Backup files are not needed, -# because we have git ;-) -_UpgradeReport_Files/ -Backup*/ -UpgradeLog*.XML -UpgradeLog*.htm -ServiceFabricBackup/ -*.rptproj.bak - -# SQL Server files -*.mdf -*.ldf -*.ndf - -# Business Intelligence projects -*.rdl.data -*.bim.layout -*.bim_*.settings -*.rptproj.rsuser -*- [Bb]ackup.rdl -*- [Bb]ackup ([0-9]).rdl -*- [Bb]ackup ([0-9][0-9]).rdl - -# Microsoft Fakes -FakesAssemblies/ - -# GhostDoc plugin setting file -*.GhostDoc.xml - -# Node.js Tools for Visual Studio -.ntvs_analysis.dat -node_modules/ - -# Visual Studio 6 build log -*.plg - -# Visual Studio 6 workspace options file -*.opt - -# Visual Studio 6 auto-generated workspace file (contains which files were open etc.) -*.vbw - -# Visual Studio LightSwitch build output -**/*.HTMLClient/GeneratedArtifacts -**/*.DesktopClient/GeneratedArtifacts -**/*.DesktopClient/ModelManifest.xml -**/*.Server/GeneratedArtifacts -**/*.Server/ModelManifest.xml -_Pvt_Extensions - -# Paket dependency manager -.paket/paket.exe -paket-files/ - -# FAKE - F# Make -.fake/ - -# CodeRush personal settings -.cr/personal - -# Python Tools for Visual Studio (PTVS) -__pycache__/ -*.pyc - -# Cake - Uncomment if you are using it -# tools/** -# !tools/packages.config - -# Tabs Studio -*.tss - -# Telerik's JustMock configuration file -*.jmconfig - -# BizTalk build output -*.btp.cs -*.btm.cs -*.odx.cs -*.xsd.cs - -# OpenCover UI analysis results -OpenCover/ - -# Azure Stream Analytics local run output -ASALocalRun/ - -# MSBuild Binary and Structured Log -*.binlog - -# NVidia Nsight GPU debugger configuration file -*.nvuser - -# MFractors (Xamarin productivity tool) working folder -.mfractor/ - -# Local History for Visual Studio -.localhistory/ - -# BeatPulse healthcheck temp database -healthchecksdb - -# Backup folder for Package Reference Convert tool in Visual Studio 2017 -MigrationBackup/ - -# Ionide (cross platform F# VS Code tools) working folder -.ionide/ - -# Fody - auto-generated XML schema -FodyWeavers.xsd diff --git a/native/MagnetoLib/Magneto.cpp b/native/MagnetoLib/Magneto.cpp deleted file mode 100644 index ed1758a5e..000000000 --- a/native/MagnetoLib/Magneto.cpp +++ /dev/null @@ -1,2349 +0,0 @@ -// magneto magnetometer calibration code -// from http://sailboatinstruments.blogspot.com/2011/08/improved-magnetometer-calibration.html -// tested and works with Code::Blocks 10.05 and 16.01 -// Command line version slightly modified from original, sjames_remington@gmail.com - -#include "stdafx.h" -#include "magneto.h" - -// -// *** CODE STARTS HERE *** -// -int main() -{ - int nlines = 0, nlines2 = 0; - char buf[120]; - double *D, *S, *C, *S11, *S12, *S12t, *S22, *S22_1, *S22a, *S22b, *SS, *E, *d, *U, *SSS; - double *eigen_real, *eigen_imag, *v1, *v2, *v, *Q, *Q_1, *B, *QB, J, hmb, *SSSS; - int *p; - int i, j, index; - double maxval, norm, btqb, *eigen_real3, *eigen_imag3, *Dz, *vdz, *SQ, *A_1, hm, norm1, norm2, norm3; - double x, y, z, x2, nxsrej, xs, xave; - double *raw; //raw obs - char filename[64]; - FILE *fp, *fp2; - - printf("\r\nMagneto 1.3 12/25/2020\r\nInput .csv file? "); - scanf("%s", &filename); - - fp = fopen(filename, "r"); - if (fp == NULL) { printf("file not found"); return 0; } - // get the number of lines in the file - while (fgets(buf, 100, fp) != NULL) - nlines++; - rewind(fp); - - // - // calculate mean (norm) and standard deviation for possible outlier rejection - // - - xs = 0; - xave = 0; - for (i = 0; i < nlines; i++) - { - fgets(buf, 100, fp); - index = sscanf(buf, "%lf,%lf,%lf", &x, &y, &z); - printf("%3d %6.0f %6.0f %6.0f\n", i, x, y, z); - x2 = x*x + y*y + z*z; - xs += x2; - xave += sqrt(x2); - } - xave = xave / nlines; //mean vector length - xs = sqrt(xs / nlines - (xave*xave)); //std. dev. - rewind(fp); - - // summarize statistics, give opportunity to reject outlier measurements - - printf("\r\nAverage magnitude (sigma) of %d vectors (default Hm) = %6.1lf (%6.1lf)\r\n", nlines, xave, xs); - printf("\r\nReject outliers? (0 or n, reject if > n * sigma) "); - scanf("%lf", &nxsrej); - printf(" rejection level selected: %lf\r\n ", nxsrej); - - // scan file again - - nlines2 = 0; //count non-rejected - if (nxsrej > 0) { - printf("\r\nRejecting measurements if abs(vector_length-average)/(std. dev.) > %5.1lf", nxsrej); - - // outlier rejection, count them - for (i = 0; i < nlines; i++) - { - fgets(buf, 100, fp); - index = sscanf(buf, "%lf,%lf,%lf", &x, &y, &z); - // printf("%f %f %f\n",x,y,z); - x2 = sqrt(x*x + y*y + z*z); //vector length - x2 = fabs(x2 - xave) / xs; //standard deviations from mean - if (x2 < nxsrej) nlines2++; - } - printf("\r\n%3d measurements will be rejected", nlines - nlines2); - rewind(fp); - } - - // third time through! - - if (nlines2 == 0) nlines2 = nlines; //none rejected - j = 0; //index for good measurements - - // allocate array space for accepted measurements - D = (double*)malloc(10 * nlines2 * sizeof(double)); - raw = (double*)malloc(3 * nlines2 * sizeof(double)); - - for (i = 0; i < nlines; i++) - { - fgets(buf, 100, fp); - index = sscanf(buf, "%lf,%lf,%lf", &x, &y, &z); //comma separated values - // printf("%6.1f %6.1f %6.1f\n",x,y,z); - x2 = sqrt(x*x + y*y + z*z); //vector length - x2 = fabs(x2 - xave) / xs; //standard deviations from mean - if ((nxsrej == 0) || (x2 < nxsrej)) { - - raw[3 * j] = x; - raw[3 * j + 1] = y; - raw[3 * j + 2] = z; - D[j] = x * x; - D[nlines2 + j] = y * y; - D[nlines2 * 2 + j] = z * z; - D[nlines2 * 3 + j] = 2.0 * y * z; - D[nlines2 * 4 + j] = 2.0 * x * z; - D[nlines2 * 5 + j] = 2.0 * x * y; - D[nlines2 * 6 + j] = 2.0 * x; - D[nlines2 * 7 + j] = 2.0 * y; - D[nlines2 * 8 + j] = 2.0 * z; - D[nlines2 * 9 + j] = 1.0; - j++; //index good measurements - } - } - fclose(fp); - printf("\r\n%3d measurements processed, expected %d", j, nlines2); - nlines = nlines2; //reset number of measurements - - printf("\r\nExpected norm of local field vector Hm? (0 for default above) "); - scanf("%lf", &hm); - - if (hm == 0.0) hm = xave; - - // allocate memory for matrix S - S = (double*)malloc(10 * 10 * sizeof(double)); - Multiply_Self_Transpose(S, D, 10, nlines); - - // Create pre-inverted constraint matrix C - C = (double*)malloc(6 * 6 * sizeof(double)); - C[0] = 0.0; C[1] = 0.5; C[2] = 0.5; C[3] = 0.0; C[4] = 0.0; C[5] = 0.0; - C[6] = 0.5; C[7] = 0.0; C[8] = 0.5; C[9] = 0.0; C[10] = 0.0; C[11] = 0.0; - C[12] = 0.5; C[13] = 0.5; C[14] = 0.0; C[15] = 0.0; C[16] = 0.0; C[17] = 0.0; - C[18] = 0.0; C[19] = 0.0; C[20] = 0.0; C[21] = -0.25; C[22] = 0.0; C[23] = 0.0; - C[24] = 0.0; C[25] = 0.0; C[26] = 0.0; C[27] = 0.0; C[28] = -0.25; C[29] = 0.0; - C[30] = 0.0; C[31] = 0.0; C[32] = 0.0; C[33] = 0.0; C[34] = 0.0; C[35] = -0.25; - - S11 = (double*)malloc(6 * 6 * sizeof(double)); - Get_Submatrix(S11, 6, 6, S, 10, 0, 0); - S12 = (double*)malloc(6 * 4 * sizeof(double)); - Get_Submatrix(S12, 6, 4, S, 10, 0, 6); - S12t = (double*)malloc(4 * 6 * sizeof(double)); - Get_Submatrix(S12t, 4, 6, S, 10, 6, 0); - S22 = (double*)malloc(4 * 4 * sizeof(double)); - Get_Submatrix(S22, 4, 4, S, 10, 6, 6); - - S22_1 = (double*)malloc(4 * 4 * sizeof(double)); - for (i = 0; i < 16; i++) - S22_1[i] = S22[i]; - Choleski_LU_Decomposition(S22_1, 4); - Choleski_LU_Inverse(S22_1, 4); - - // Calculate S22a = S22_1 * S12t 4*6 = 4x4 * 4x6 C = AB - S22a = (double*)malloc(4 * 6 * sizeof(double)); - Multiply_Matrices(S22a, S22_1, 4, 4, S12t, 6); - - // Then calculate S22b = S12 * S22a ( 6x6 = 6x4 * 4x6) - S22b = (double*)malloc(6 * 6 * sizeof(double)); - Multiply_Matrices(S22b, S12, 6, 4, S22a, 6); - - // Calculate SS = S11 - S22b - SS = (double*)malloc(6 * 6 * sizeof(double)); - for (i = 0; i < 36; i++) - SS[i] = S11[i] - S22b[i]; - E = (double*)malloc(6 * 6 * sizeof(double)); - Multiply_Matrices(E, C, 6, 6, SS, 6); - - SSS = (double*)malloc(6 * 6 * sizeof(double)); - Hessenberg_Form_Elementary(E, SSS, 6); - - eigen_real = (double*)malloc(6 * sizeof(double)); - eigen_imag = (double*)malloc(6 * sizeof(double)); - - QR_Hessenberg_Matrix(E, SSS, eigen_real, eigen_imag, 6, 100); - - index = 0; - maxval = eigen_real[0]; - for (i = 1; i < 6; i++) - { - if (eigen_real[i] > maxval) - { - maxval = eigen_real[i]; - index = i; - } - } - - v1 = (double*)malloc(6 * sizeof(double)); - - v1[0] = SSS[index]; - v1[1] = SSS[index + 6]; - v1[2] = SSS[index + 12]; - v1[3] = SSS[index + 18]; - v1[4] = SSS[index + 24]; - v1[5] = SSS[index + 30]; - - // normalize v1 - norm = sqrt(v1[0] * v1[0] + v1[1] * v1[1] + v1[2] * v1[2] + v1[3] * v1[3] + v1[4] * v1[4] + v1[5] * v1[5]); - v1[0] /= norm; - v1[1] /= norm; - v1[2] /= norm; - v1[3] /= norm; - v1[4] /= norm; - v1[5] /= norm; - - if (v1[0] < 0.0) - { - v1[0] = -v1[0]; - v1[1] = -v1[1]; - v1[2] = -v1[2]; - v1[3] = -v1[3]; - v1[4] = -v1[4]; - v1[5] = -v1[5]; - } - - // Calculate v2 = S22a * v1 ( 4x1 = 4x6 * 6x1) - v2 = (double*)malloc(4 * sizeof(double)); - Multiply_Matrices(v2, S22a, 4, 6, v1, 1); - - v = (double*)malloc(10 * sizeof(double)); - - v[0] = v1[0]; - v[1] = v1[1]; - v[2] = v1[2]; - v[3] = v1[3]; - v[4] = v1[4]; - v[5] = v1[5]; - v[6] = -v2[0]; - v[7] = -v2[1]; - v[8] = -v2[2]; - v[9] = -v2[3]; - - Q = (double*)malloc(3 * 3 * sizeof(double)); - - Q[0] = v[0]; - Q[1] = v[5]; - Q[2] = v[4]; - Q[3] = v[5]; - Q[4] = v[1]; - Q[5] = v[3]; - Q[6] = v[4]; - Q[7] = v[3]; - Q[8] = v[2]; - - U = (double*)malloc(3 * sizeof(double)); - - U[0] = v[6]; - U[1] = v[7]; - U[2] = v[8]; - Q_1 = (double*)malloc(3 * 3 * sizeof(double)); - for (i = 0; i < 9; i++) - Q_1[i] = Q[i]; - Choleski_LU_Decomposition(Q_1, 3); - Choleski_LU_Inverse(Q_1, 3); - - // Calculate B = Q-1 * U ( 3x1 = 3x3 * 3x1) - B = (double*)malloc(3 * sizeof(double)); - Multiply_Matrices(B, Q_1, 3, 3, U, 1); - B[0] = -B[0]; // x-axis combined bias - B[1] = -B[1]; // y-axis combined bias - B[2] = -B[2]; // z-axis combined bias - - // for(i = 0; i < 3; i++) - printf("\r\nCombined bias vector B:\r\n"); - printf("%8.2lf %8.2lf %8.2lf \r\n", B[0], B[1], B[2]); - - // First calculate QB = Q * B ( 3x1 = 3x3 * 3x1) - QB = (double*)malloc(3 * sizeof(double)); - Multiply_Matrices(QB, Q, 3, 3, B, 1); - - // Then calculate btqb = BT * QB ( 1x1 = 1x3 * 3x1) - Multiply_Matrices(&btqb, B, 1, 3, QB, 1); - - // Calculate hmb = sqrt(btqb - J). - J = v[9]; - hmb = sqrt(btqb - J); - - // Calculate SQ, the square root of matrix Q - SSSS = (double*)malloc(3 * 3 * sizeof(double)); - Hessenberg_Form_Elementary(Q, SSSS, 3); - - eigen_real3 = (double*)malloc(3 * sizeof(double)); - eigen_imag3 = (double*)malloc(3 * sizeof(double)); - QR_Hessenberg_Matrix(Q, SSSS, eigen_real3, eigen_imag3, 3, 100); - - // normalize eigenvectors - norm1 = sqrt(SSSS[0] * SSSS[0] + SSSS[3] * SSSS[3] + SSSS[6] * SSSS[6]); - SSSS[0] /= norm1; - SSSS[3] /= norm1; - SSSS[6] /= norm1; - norm2 = sqrt(SSSS[1] * SSSS[1] + SSSS[4] * SSSS[4] + SSSS[7] * SSSS[7]); - SSSS[1] /= norm2; - SSSS[4] /= norm2; - SSSS[7] /= norm2; - norm3 = sqrt(SSSS[2] * SSSS[2] + SSSS[5] * SSSS[5] + SSSS[8] * SSSS[8]); - SSSS[2] /= norm3; - SSSS[5] /= norm3; - SSSS[8] /= norm3; - - Dz = (double*)malloc(3 * 3 * sizeof(double)); - for (i = 0; i < 9; i++) - Dz[i] = 0.0; - Dz[0] = sqrt(eigen_real3[0]); - Dz[4] = sqrt(eigen_real3[1]); - Dz[8] = sqrt(eigen_real3[2]); - - vdz = (double*)malloc(3 * 3 * sizeof(double)); - Multiply_Matrices(vdz, SSSS, 3, 3, Dz, 3); - - Transpose_Square_Matrix(SSSS, 3); - - SQ = (double*)malloc(3 * 3 * sizeof(double)); - Multiply_Matrices(SQ, vdz, 3, 3, SSSS, 3); - - // hm = 0.569; - A_1 = (double*)malloc(3 * 3 * sizeof(double)); - - for (i = 0; i < 9; i++) - A_1[i] = SQ[i] * hm / hmb; - - printf("\r\nCorrection matrix Ainv, using Hm=%lf:\r\n", hm); - for (i = 0; i < 3; i++) - printf("%9.5lf %9.5lf %9.5lf\r\n", A_1[i * 3], A_1[i * 3 + 1], A_1[i * 3 + 2]); - printf("\r\nWhere Hcalc = Ainv*(H-B)\r\n"); - - printf("\r\nCode initialization statements...\r\n"); - printf("\r\n float B[3]\r\n {%8.2lf,%8.2lf,%8.2lf};\r\n", B[0], B[1], B[2]); - printf("\r\n float Ainv[3][3]\r\n {{%9.5lf,%9.5lf,%9.5lf},\r\n", A_1[0], A_1[1], A_1[2]); - printf(" {%9.5lf,%9.5lf,%9.5lf},\r\n", A_1[3], A_1[4], A_1[5]); - printf(" {%9.5lf,%9.5lf,%9.5lf}};\r\n", A_1[6], A_1[7], A_1[8]); - - - // dump corrected measurements if desired - - printf(" output filename for corrected values (csv) "); - scanf("%s", &filename); - if (strlen(filename) > 1) { - - fp2 = fopen(filename, "w"); - float xc, yc, zc; - xs = 0; - for (i = 0; i // required for memcpy() - -void Copy_Vector(double *d, double *s, int n) -{ - memcpy(d, s, sizeof(double) * n); -} -//////////////////////////////////////////////////////////////////////////////// -// File: multiply_self_transpose.c // -// Routine(s): // -// Multiply_Self_Transpose // -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -// void Multiply_Self_Transpose(double *C, double *A, int nrows, int ncols ) // -// // -// Description: // -// Post multiply an nrows x ncols matrix A by its transpose. The result // -// is an nrows x nrows square symmetric matrix C, i.e. C = A A', where ' // -// denotes the transpose. // -// The matrix C should be declared as double C[nrows][nrows] in the // -// calling routine. The memory allocated to C should not include any // -// memory allocated to A. // -// // -// Arguments: // -// double *C Pointer to the first element of the matrix C. // -// double *A Pointer to the first element of the matrix A. // -// int nrows The number of rows of matrix A. // -// int ncols The number of columns of the matrices A. // -// // -// Return Values: // -// void // -// // -// Example: // -// #define N // -// #define M // -// double A[M][N], C[M][M]; // -// // -// (your code to initialize the matrix A) // -// // -// Multiply_Self_Transpose(&C[0][0], &A[0][0], M, N); // -// printf("The matrix C = AA ' is \n"); ... // -//////////////////////////////////////////////////////////////////////////////// -void Multiply_Self_Transpose(double *C, double *A, int nrows, int ncols) -{ - int i, j, k; - double *pA = {}; - double *p_A = A; - double *pB; - double *pCdiag = C; - double *pC = C; - double *pCt; - - for (i = 0; i < nrows; pCdiag += nrows + 1, p_A = pA, i++) { - pC = pCdiag; - pCt = pCdiag; - pB = p_A; - for (j = i; j < nrows; pC++, pCt += nrows, j++) { - pA = p_A; - *pC = 0.0; - for (k = 0; k < ncols; k++) *pC += *(pA++) * *(pB++); - *pCt = *pC; - } - } -} -//////////////////////////////////////////////////////////////////////////////// -// File: get_submatrix.c // -// Routine(s): // -// Get_Submatrix // -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -// void Get_Submatrix(double *S, int mrows, int mcols, // -// double *A, int ncols, int row, int col) // -// // -// Description: // -// Copy the mrows and mcols of the nrows x ncols matrix A starting with // -// A[row][col] to the submatrix S. // -// Note that S should be declared double S[mrows][mcols] in the calling // -// routine. // -// // -// Arguments: // -// double *S Destination address of the submatrix. // -// int mrows The number of rows of the matrix S. // -// int mcols The number of columns of the matrix S. // -// double *A Pointer to the first element of the matrix A[nrows][ncols]// -// int ncols The number of columns of the matrix A. // -// int row The row of A corresponding to the first row of S. // -// int col The column of A corresponding to the first column of S. // -// // -// Return Values: // -// void // -// // -// Example: // -// #define N // -// #define M // -// #define NB // -// #define MB // -// double A[M][N], B[MB][NB]; // -// int row, col; // -// // -// (your code to set the matrix A, the row number row and column number // -// col) // -// // -// if ( (row >= 0) && (col >= 0) && ((row + MB) < M) && ((col + NB) < N) )// -// Get_Submatrix(&B[0][0], MB, NB, &A[0][0], N, row, col); // -// printf("The submatrix B is \n"); ... } // -//////////////////////////////////////////////////////////////////////////////// - -#include // required for memcpy() - -void Get_Submatrix(double *S, int mrows, int mcols, - double *A, int ncols, int row, int col) -{ - int number_of_bytes = sizeof(double) * mcols; - - for (A += row * ncols + col; mrows > 0; A += ncols, S += mcols, mrows--) - memcpy(S, A, number_of_bytes); -} -//////////////////////////////////////////////////////////////////////////////// -// File: choleski.c // -// Contents: // -// Choleski_LU_Decomposition // -// Choleski_LU_Solve // -// Choleski_LU_Inverse // -// // -// Required Externally Defined Routines: // -// Lower_Triangular_Solve // -// Lower_Triangular_Inverse // -// Upper_Triangular_Solve // -//////////////////////////////////////////////////////////////////////////////// - -#include // required for sqrt() - -// Required Externally Defined Routines -int Lower_Triangular_Solve(double *L, double B[], double x[], int n); -int Lower_Triangular_Inverse(double *L, int n); -int Upper_Triangular_Solve(double *U, double B[], double x[], int n); - -//////////////////////////////////////////////////////////////////////////////// -// int Choleski_LU_Decomposition(double *A, int n) // -// // -// Description: // -// This routine uses Choleski's method to decompose the n x n positive // -// definite symmetric matrix A into the product of a lower triangular // -// matrix L and an upper triangular matrix U equal to the transpose of L. // -// The original matrix A is replaced by L and U with L stored in the // -// lower triangular part of A and the transpose U in the upper triangular // -// part of A. The original matrix A is therefore destroyed. // -// // -// Choleski's decomposition is performed by evaluating, in order, the // -// following pair of expressions for k = 0, ... ,n-1 : // -// L[k][k] = sqrt( A[k][k] - ( L[k][0] ^ 2 + ... + L[k][k-1] ^ 2 ) ) // -// L[i][k] = (A[i][k] - (L[i][0]*L[k][0] + ... + L[i][k-1]*L[k][k-1])) // -// / L[k][k] // -// and subsequently setting // -// U[k][i] = L[i][k], for i = k+1, ... , n-1. // -// // -// After performing the LU decomposition for A, call Choleski_LU_Solve // -// to solve the equation Ax = B or call Choleski_LU_Inverse to calculate // -// the inverse of A. // -// // -// Arguments: // -// double *A On input, the pointer to the first element of the matrix // -// A[n][n]. On output, the matrix A is replaced by the lower // -// and upper triangular Choleski factorizations of A. // -// int n The number of rows and/or columns of the matrix A. // -// // -// Return Values: // -// 0 Success // -// -1 Failure - The matrix A is not positive definite symmetric (within // -// working accuracy). // -// // -// Example: // -// #define N // -// double A[N][N]; // -// // -// (your code to initialize the matrix A) // -// err = Choleski_LU_Decomposition((double *) A, N); // -// if (err < 0) printf(" Matrix A is singular\n"); // -// else { printf(" The LLt decomposition of A is \n"); // -// ... // -//////////////////////////////////////////////////////////////////////////////// -// // -int Choleski_LU_Decomposition(double *A, int n) -{ - int i, k, p; - double *p_Lk0; // pointer to L[k][0] - double *p_Lkp; // pointer to L[k][p] - double *p_Lkk; // pointer to diagonal element on row k. - double *p_Li0; // pointer to L[i][0] - double reciprocal; - - for (k = 0, p_Lk0 = A; k < n; p_Lk0 += n, k++) { - - // Update pointer to row k diagonal element. - - p_Lkk = p_Lk0 + k; - - // Calculate the difference of the diagonal element in row k - // from the sum of squares of elements row k from column 0 to - // column k-1. - - for (p = 0, p_Lkp = p_Lk0; p < k; p_Lkp += 1, p++) - *p_Lkk -= *p_Lkp * *p_Lkp; - - // If diagonal element is not positive, return the error code, - // the matrix is not positive definite symmetric. - - if (*p_Lkk <= 0.0) return -1; - - // Otherwise take the square root of the diagonal element. - - *p_Lkk = sqrt(*p_Lkk); - reciprocal = 1.0 / *p_Lkk; - - // For rows i = k+1 to n-1, column k, calculate the difference - // between the i,k th element and the inner product of the first - // k-1 columns of row i and row k, then divide the difference by - // the diagonal element in row k. - // Store the transposed element in the upper triangular matrix. - - p_Li0 = p_Lk0 + n; - for (i = k + 1; i < n; p_Li0 += n, i++) { - for (p = 0; p < k; p++) - *(p_Li0 + k) -= *(p_Li0 + p) * *(p_Lk0 + p); - *(p_Li0 + k) *= reciprocal; - *(p_Lk0 + i) = *(p_Li0 + k); - } - } - return 0; -} - - -//////////////////////////////////////////////////////////////////////////////// -// int Choleski_LU_Solve(double *LU, double *B, double *x, int n) // -// // -// Description: // -// This routine uses Choleski's method to solve the linear equation // -// Ax = B. This routine is called after the matrix A has been decomposed // -// into a product of a lower triangular matrix L and an upper triangular // -// matrix U which is the transpose of L. The matrix A is the product LU. // -// The solution proceeds by solving the linear equation Ly = B for y and // -// subsequently solving the linear equation Ux = y for x. // -// // -// Arguments: // -// double *LU Pointer to the first element of the matrix whose elements // -// form the lower and upper triangular matrix factors of A. // -// double *B Pointer to the column vector, (n x 1) matrix, B // -// double *x Solution to the equation Ax = B. // -// int n The number of rows and/or columns of the matrix LU. // -// // -// Return Values: // -// 0 Success // -// -1 Failure - The matrix L is singular. // -// // -// Example: // -// #define N // -// double A[N][N], B[N], x[N]; // -// // -// (your code to create matrix A and column vector B) // -// err = Choleski_LU_Decomposition(&A[0][0], N); // -// if (err < 0) printf(" Matrix A is singular\n"); // -// else { // -// err = Choleski_LU_Solve(&A[0][0], B, x, n); // -// if (err < 0) printf(" Matrix A is singular\n"); // -// else printf(" The solution is \n"); // -// ... // -// } // -//////////////////////////////////////////////////////////////////////////////// -// // -int Choleski_LU_Solve(double *LU, double B[], double x[], int n) -{ - - // Solve the linear equation Ly = B for y, where L is a lower - // triangular matrix. - - if (Lower_Triangular_Solve(LU, B, x, n) < 0) return -1; - - // Solve the linear equation Ux = y, where y is the solution - // obtained above of Ly = B and U is an upper triangular matrix. - - return Upper_Triangular_Solve(LU, x, x, n); -} - - -//////////////////////////////////////////////////////////////////////////////// -// int Choleski_LU_Inverse(double *LU, int n) // -// // -// Description: // -// This routine uses Choleski's method to find the inverse of the matrix // -// A. This routine is called after the matrix A has been decomposed // -// into a product of a lower triangular matrix L and an upper triangular // -// matrix U which is the transpose of L. The matrix A is the product of // -// the L and U. Upon completion, the inverse of A is stored in LU so // -// that the matrix LU is destroyed. // -// // -// Arguments: // -// double *LU On input, the pointer to the first element of the matrix // -// whose elements form the lower and upper triangular matrix // -// factors of A. On output, the matrix LU is replaced by the // -// inverse of the matrix A equal to the product of L and U. // -// int n The number of rows and/or columns of the matrix LU. // -// // -// Return Values: // -// 0 Success // -// -1 Failure - The matrix L is singular. // -// // -// Example: // -// #define N // -// double A[N][N], B[N], x[N]; // -// // -// (your code to create matrix A and column vector B) // -// err = Choleski_LU_Decomposition(&A[0][0], N); // -// if (err < 0) printf(" Matrix A is singular\n"); // -// else { // -// err = Choleski_LU_Inverse(&A[0][0], n); // -// if (err < 0) printf(" Matrix A is singular\n"); // -// else printf(" The inverse is \n"); // -// ... // -// } // -//////////////////////////////////////////////////////////////////////////////// -// // -int Choleski_LU_Inverse(double *LU, int n) -{ - int i, j, k; - double *p_i, *p_j, *p_k; - double sum; - - if (Lower_Triangular_Inverse(LU, n) < 0) return -1; - - // Premultiply L inverse by the transpose of L inverse. - - for (i = 0, p_i = LU; i < n; i++, p_i += n) { - for (j = 0, p_j = LU; j <= i; j++, p_j += n) { - sum = 0.0; - for (k = i, p_k = p_i; k < n; k++, p_k += n) - sum += *(p_k + i) * *(p_k + j); - *(p_i + j) = sum; - *(p_j + i) = sum; - } - } - - return 0; -} -//////////////////////////////////////////////////////////////////////////////// -// File: multiply_matrices.c // -// Routine(s): // -// Multiply_Matrices // -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -// void Multiply_Matrices(double *C, double *A, int nrows, int ncols, // -// double *B, int mcols) // -// // -// Description: // -// Post multiply the nrows x ncols matrix A by the ncols x mcols matrix // -// B to form the nrows x mcols matrix C, i.e. C = A B. // -// The matrix C should be declared as double C[nrows][mcols] in the // -// calling routine. The memory allocated to C should not include any // -// memory allocated to A or B. // -// // -// Arguments: // -// double *C Pointer to the first element of the matrix C. // -// double *A Pointer to the first element of the matrix A. // -// int nrows The number of rows of the matrices A and C. // -// int ncols The number of columns of the matrices A and the // -// number of rows of the matrix B. // -// double *B Pointer to the first element of the matrix B. // -// int mcols The number of columns of the matrices B and C. // -// // -// Return Values: // -// void // -// // -// Example: // -// #define N // -// #define M // -// #define NB // -// double A[M][N], B[N][NB], C[M][NB]; // -// // -// (your code to initialize the matrices A and B) // -// // -// Multiply_Matrices(&C[0][0], &A[0][0], M, N, &B[0][0], NB); // -// printf("The matrix C is \n"); ... // -//////////////////////////////////////////////////////////////////////////////// -void Multiply_Matrices(double *C, double *A, int nrows, int ncols, - double *B, int mcols) -{ - double *pB; - double *p_B; - int i, j, k; - - for (i = 0; i < nrows; A += ncols, i++) - for (p_B = B, j = 0; j < mcols; C++, p_B++, j++) { - pB = p_B; - *C = 0.0; - for (k = 0; k < ncols; pB += mcols, k++) - *C += *(A + k) * *pB; - } -} -//////////////////////////////////////////////////////////////////////////////// -// File: identity_matrix.c // -// Routine(s): // -// Identity_Matrix // -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -// void Identity_Matrix(double *A, int n) // -// // -// Description: // -// Set the square n x n matrix A equal to the identity matrix, i.e. // -// A[i][j] = 0 if i != j and A[i][i] = 1. // -// // -// Arguments: // -// double *A Pointer to the first element of the matrix A. // -// int n The number of rows and columns of the matrix A. // -// // -// Return Values: // -// void // -// // -// Example: // -// #define N // -// double A[N][N]; // -// // -// Identity_Matrix(&A[0][0], N); // -// printf("The matrix A is \n"); ... // -//////////////////////////////////////////////////////////////////////////////// -void Identity_Matrix(double *A, int n) -{ - int i, j; - - for (i = 0; i < n - 1; i++) { - *A++ = 1.0; - for (j = 0; j < n; j++) *A++ = 0.0; - } - *A = 1.0; -} -//////////////////////////////////////////////////////////////////////////////// -// File: hessenberg_elementary.c // -// Routine(s): // -// Hessenberg_Form_Elementary // -// Hessenberg_Elementary_Transform // -//////////////////////////////////////////////////////////////////////////////// - -#include // required for malloc() and free() -#include // required for fabs() - -// Required Externally Defined Routines -void Interchange_Rows(double *A, int row1, int row2, int ncols); -void Interchange_Columns(double *A, int col1, int col2, int nrows, int ncols); -void Identity_Matrix(double *A, int n); -void Copy_Vector(double *d, double *s, int n); - -// Internally Defined Routines -static void Hessenberg_Elementary_Transform(double* H, double *S, - int perm[], int n); - -//////////////////////////////////////////////////////////////////////////////// -// int Hessenberg_Form_Elementary(double *A, double *S, int n) // -// // -// Description: // -// This program transforms the square matrix A to a similar matrix in // -// Hessenberg form by a multiplying A on the right by a sequence of // -// elementary transformations and on the left by the sequence of inverse // -// transformations. // -// Def: Two matrices A and B are said to be similar if there exists a // -// nonsingular matrix S such that A S = S B. // -// Def A Hessenberg matrix is the sum of an upper triangular matrix and // -// a matrix all of whose components are 0 except possibly on its // -// subdiagonal. A Hessenberg matrix is sometimes said to be almost // -// upper triangular. // -// The algorithm proceeds by successivly selecting columns j = 0,...,n-3 // -// and then assuming that columns 0, ..., j-1 have been reduced to Hessen-// -// berg form, for rows j+1 to n-1, select that row j' for which |a[j'][j]|// -// is a maximum and interchange rows j+1 and j' and columns j+1 and j'. // -// Then for each i = j+2 to n-1, let x = a[i][j] / a[j+1][j] subtract // -// x * row j+1 from row i and add x * column i to column j+1. // -// // -// Arguments: // -// double *A On input a pointer to the first element of the matrix // -// A[n][n]. The matrix A is replaced with the matrix H, // -// a matrix in Hessenberg form similar to A. // -// double *S On output the transform such that A S = S H. // -// The matrix S should be dimensioned at least n x n in the // -// calling routine. // -// int n The number of rows or columns of the matrix A. // -// // -// Return Values: // -// 0 Success // -// -1 Failure - Not enough memory // -// // -// Example: // -// #define N // -// double A[N][N], S[N][N]; // -// // -// (your code to create the matrix A) // -// if (Hessenberg_Form_Elementary(&A[0][0], (double*)S, N ) < 0) { // -// printf("Not enough memory\n"); exit(0); // -// } // -// // -//////////////////////////////////////////////////////////////////////////////// -// // -int Hessenberg_Form_Elementary(double *A, double* S, int n) -{ - int i, j, k, col, row; - int* perm; - double *p_row, *pS_row; - double max; - double s; - double *pA, *pB, *pC, *pS; - - // n x n matrices for which n <= 2 are already in Hessenberg form - - if (n <= 1) { *S = 1.0; return 0; } - if (n == 2) { *S++ = 1.0; *S++ = 0.0; *S++ = 1.0; *S = 0.0; return 0; } - - // Allocate working memory - - perm = (int*)malloc(n * sizeof(int)); - if (perm == NULL) return -1; // not enough memory - - // For each column use Elementary transformations - // to zero the entries below the subdiagonal. - - p_row = A + n; - pS_row = S + n; - for (col = 0; col < (n - 2); p_row += n, pS_row += n, col++) { - - // Find the row in column "col" with maximum magnitude where - // row >= col + 1. - - row = col + 1; - perm[row] = row; - for (pA = p_row + col, max = 0.0, i = row; i < n; pA += n, i++) - if (fabs(*pA) > max) { perm[row] = i; max = fabs(*pA); } - - // If perm[row] != row, then interchange row "row" and row - // perm[row] and interchange column "row" and column perm[row]. - - if (perm[row] != row) { - Interchange_Rows(A, row, perm[row], n); - Interchange_Columns(A, row, perm[row], n, n); - } - - // Zero out the components lying below the subdiagonal. - - pA = p_row + n; - pS = pS_row + n; - for (i = col + 2; i < n; pA += n, pS += n, i++) { - s = *(pA + col) / *(p_row + col); - for (j = 0; j < n; j++) - *(pA + j) -= *(p_row + j) * s; - *(pS + col) = s; - for (j = 0, pB = A + col + 1, pC = A + i; j < n; pB += n, pC += n, j++) - *pB += s * *pC; - } - } - pA = A + n + n; - pS = S + n + n; - for (i = 2; i < n; pA += n, pS += n, i++) Copy_Vector(pA, pS, i - 1); - - Hessenberg_Elementary_Transform(A, S, perm, n); - - free(perm); - return 0; -} - - -//////////////////////////////////////////////////////////////////////////////// -// static void Hessenberg_Elementary_Transform(double* H, double *S, // -// int perm[], int n) // -// // -// Description: // -// Given a n x n matrix A, let H be the matrix in Hessenberg form similar // -// to A, i.e. A S = S H. If v is an eigenvector of H with eigenvalue z, // -// i.e. Hv = zv, then ASv = SHv = z Sv, i.e. Sv is the eigenvector of A // -// with corresponding eigenvalue z. // -// This routine returns S where S is the similarity transformation such // -// that A S = S H. // -// // -// Arguments: // -// double* H On input a matrix in Hessenberg form with transformation // -// elements stored below the subdiagonal part. // -// On output the matrix in Hessenberg form with elements // -// below the subdiagonal zeroed out. // -// double* S On output, the transformations matrix such that // -// A S = S H. // -// int perm[] Array of row/column interchanges. // -// int n The order of the matrices H and S. // -// // -// Return Values: // -// void // -// // -//////////////////////////////////////////////////////////////////////////////// -// // -static void Hessenberg_Elementary_Transform(double *H, double* S, int perm[], - int n) -{ - int i, j; - double *pS, *pH; - double x; - - Identity_Matrix(S, n); - for (i = n - 2; i >= 1; i--) { - pH = H + n * (i + 1); - pS = S + n * (i + 1); - for (j = i + 1; j < n; pH += n, pS += n, j++) { - *(pS + i) = *(pH + i - 1); - *(pH + i - 1) = 0.0; - } - if (perm[i] != i) { - pS = S + n * i; - pH = S + n * perm[i]; - for (j = i; j < n; j++) { - *(pS + j) = *(pH + j); - *(pH + j) = 0.0; - } - *(pH + i) = 1.0; - } - } -} -//////////////////////////////////////////////////////////////////////////////// -// File: qr_hessenberg_matrix.c // -// Routine(s): // -// QR_Hessenberg_Matrix // -//////////////////////////////////////////////////////////////////////////////// - -#include // required for fabs() and sqrt() -#include // required for DBL_EPSILON - - -// Internally Defined Routines -static void One_Real_Eigenvalue(double Hrow[], double eigen_real[], - double eigen_imag[], int row, double shift); -static void Two_Eigenvalues(double *H, double *S, double eigen_real[], - double eigen_imag[], int n, int k, double t); -static void Update_Row(double *Hrow, double cos, double sin, int n, int k); -static void Update_Column(double* H, double cos, double sin, int n, int k); -static void Update_Transformation(double *S, double cos, double sin, - int n, int k); -static void Double_QR_Iteration(double *H, double *S, int row, int min_row, - int n, double* shift, int iteration); -static void Product_and_Sum_of_Shifts(double *H, int n, int max_row, - double* shift, double *trace, double *det, int iteration); -static int Two_Consecutive_Small_Subdiagonal(double* H, int min_row, - int max_row, int n, double trace, double det); -static void Double_QR_Step(double *H, int min_row, int max_row, int min_col, - double trace, double det, double *S, int n); -static void Complex_Division(double x, double y, double u, double v, - double* a, double* b); -static void BackSubstitution(double *H, double eigen_real[], - double eigen_imag[], int n); -static void BackSubstitute_Real_Vector(double *H, double eigen_real[], - double eigen_imag[], int row, double zero_tolerance, int n); -static void BackSubstitute_Complex_Vector(double *H, double eigen_real[], - double eigen_imag[], int row, double zero_tolerance, int n); -static void Calculate_Eigenvectors(double *H, double *S, double eigen_real[], - double eigen_imag[], int n); - -//////////////////////////////////////////////////////////////////////////////// -// int QR_Hessenberg_Matrix( double *H, double *S, double eigen_real[], // -// double eigen_imag[], int n, int max_iteration_count) // -// // -// Description: // -// This program calculates the eigenvalues and eigenvectors of a matrix // -// in Hessenberg form. This routine is adapted from the routine 'hql2' // -// appearing in 'Handbook for Automatic Computation, vol 2: Linear // -// Algebra' published by Springer Verlag (1971) edited by Wilkinson and // -// Reinsch, Contribution II/15 Eigenvectors of Real and Complex Matrices // -// by LR and QR Triangularizations by Peters and Wilkinson. // -// // -// Arguments: // -// double *H // -// Pointer to the first element of the real n x n matrix H in upper// -// Hessenberg form. // -// double *S // -// If H is the primary data matrix, the matrix S should be set // -// to the identity n x n identity matrix on input. If H is // -// derived from an n x n matrix A, then S should be the // -// transformation matrix such that AS = SH. On output, the i-th // -// column of S corresponds to the i-th eigenvalue if that eigen- // -// value is real and the i-th and (i+1)-st columns of S correspond // -// to the i-th eigenvector with the real part in the i-th column // -// and positive imaginary part in the (i+1)-st column if that // -// eigenvalue is complex with positive imaginary part. The // -// eigenvector corresponding to the eigenvalue with negative // -// imaginary part is the complex conjugate of the eigenvector // -// corresponding to the complex conjugate of the eigenvalue. // -// If on input, S was the identity matrix, then the columns are // -// the eigenvectors of H as described, if S was a transformation // -// matrix so that AS = SH, then the columns of S are the // -// eigenvectors of A as described. // -// double eigen_real[] // -// Upon return, eigen_real[i] will contain the real part of the // -// i-th eigenvalue. // -// double eigen_imag[] // -// Upon return, eigen_ima[i] will contain the imaginary part of // -// the i-th eigenvalue. // -// int n // -// The number of rows or columns of the upper Hessenberg matrix A. // -// int max_iteration_count // -// The maximum number of iterations to try to find an eigenvalue // -// before quitting. // -// // -// Return Values: // -// 0 Success // -// -1 Failure - Unable to find an eigenvalue within 'max_iteration_count' // -// iterations. // -// // -// Example: // -// #define N // -// #define MAX_ITERATION_COUNT // -// double H[N][N], S[N][N], eigen_real[N], eigen_imag[N]; // -// int k; // -// // -// (code to initialize H[N][N] and S[N][N]) // -// k = QR_Hessenberg_Matrix( (double*)H, (double*)S, eigen_real, // -// eigen_imag, N, MAX_ITERATION_COUNT); // -// if (k < 0) {printf("Failed"); exit(1);} // -//////////////////////////////////////////////////////////////////////////////// -// // -int QR_Hessenberg_Matrix(double *H, double *S, double eigen_real[], - double eigen_imag[], int n, int max_iteration_count) -{ - int i; - int row; - int iteration; - int found_eigenvalue; - double shift = 0.0; - double* pH; - - for (row = n - 1; row >= 0; row--) { - found_eigenvalue = 0; - for (iteration = 1; iteration <= max_iteration_count; iteration++) { - - // Search for small subdiagonal element - - for (i = row, pH = H + row * n; i > 0; i--, pH -= n) - if (fabs(*(pH + i - 1)) <= DBL_EPSILON * - (fabs(*(pH - n + i - 1)) + fabs(*(pH + i)))) break; - - // If the subdiagonal element on row "row" is small, then - // that row element is an eigenvalue. If the subdiagonal - // element on row "row-1" is small, then the eigenvalues - // of the 2x2 diagonal block consisting rows "row-1" and - // "row" are eigenvalues. Otherwise perform a double QR - // iteration. - - switch (row - i) { - case 0: // One real eigenvalue - One_Real_Eigenvalue(pH, eigen_real, eigen_imag, i, shift); - found_eigenvalue = 1; - break; - case 1: // Either two real eigenvalues or a complex pair - row--; - Two_Eigenvalues(H, S, eigen_real, eigen_imag, n, row, shift); - found_eigenvalue = 1; - break; - default: - Double_QR_Iteration(H, S, i, row, n, &shift, iteration); - } - if (found_eigenvalue) break; - } - if (iteration > max_iteration_count) return -1; - } - - BackSubstitution(H, eigen_real, eigen_imag, n); - Calculate_Eigenvectors(H, S, eigen_real, eigen_imag, n); - - return 0; -} - -//////////////////////////////////////////////////////////////////////////////// -// static void One_Real_Eigenvalue( double Hrow[], double eigen_real[], // -// double eigen_imag[], int row, double shift) // -// // -// Arguments: // -// double Hrow[] // -// Pointer to the row "row" of the matrix in Hessenberg form. // -// double eigen_real[] // -// Array of the real parts of the eigenvalues. // -// double eigen_imag[] // -// Array of the imaginary parts of the eigenvalues. // -// int row // -// The row to which the pointer Hrow[] points of the matrix H. // -// double shift // -// The cumulative exceptional shift of the diagonal elements of // -// the matrix H. // -//////////////////////////////////////////////////////////////////////////////// -// // -static void One_Real_Eigenvalue(double Hrow[], double eigen_real[], - double eigen_imag[], int row, double shift) -{ - Hrow[row] += shift; - eigen_real[row] = Hrow[row]; - eigen_imag[row] = 0.0; -} - - -//////////////////////////////////////////////////////////////////////////////// -// static void Two_Eigenvalues( double *H, double *S, double eigen_real[], // -// double eigen_imag[], int n, int row, double shift) // -// // -// Description: // -// Given the 2x2 matrix A = (a[i][j]), the characteristic equation is: // -// x^2 - Tr(A) x + Det(A) = 0, where Tr(A) = a[0][0] + a[1][1] and // -// Det(A) = a[0][0] * a[1][1] - a[0][1] * a[1][0]. // -// The solution for the eigenvalues x are: // -// x1 = (Tr(A) + sqrt( (Tr(A))^2 + 4 * a[0][1] * a[1][0] ) / 2 and // -// x2 = (Tr(A) - sqrt( (Tr(A))^2 + 4 * a[0][1] * a[1][0] ) / 2. // -// Let p = (a[0][0] - a[1][1]) / 2 and q = p^2 - a[0][1] * a[1][0], then // -// x1 = a[1][1] + p [+|-] sqrt(q) and x2 = a[0][0] + a[1][1] - x1. // -// Choose the sign [+|-] to be the sign of p. // -// If q > 0.0, then both roots are real and the transformation // -// | cos sin | | a[0][0] a[0][1] | | cos -sin | // -// |-sin cos | | a[1][0] a[1][1] | | sin cos | // -// where sin = a[1][0] / r, cos = ( p + sqrt(q) ) / r, where r > 0 is // -// determined sin^2 + cos^2 = 1 transforms the matrix A to an upper // -// triangular matrix with x1 the upper diagonal element and x2 the lower.// -// If q < 0.0, then both roots form a complex conjugate pair. // -// // -// Arguments: // -// double *H // -// Pointer to the first element of the matrix in Hessenberg form. // -// double *S // -// Pointer to the first element of the transformation matrix. // -// double eigen_real[] // -// Array of the real parts of the eigenvalues. // -// double eigen_imag[] // -// Array of the imaginary parts of the eigenvalues. // -// int n // -// The dimensions of the matrix H and S. // -// int row // -// The upper most row of the block diagonal 2 x 2 submatrix of H. // -// double shift // -// The cumulative exceptional shift of the diagonal elements of // -// the matrix H. // -//////////////////////////////////////////////////////////////////////////////// -// // -static void Two_Eigenvalues(double *H, double* S, double eigen_real[], - double eigen_imag[], int n, int row, double shift) -{ - double p, q, x, discriminant, r; - double cos, sin; - double *Hrow = H + n * row; - double *Hnextrow = Hrow + n; - int nextrow = row + 1; - - p = 0.5 * (Hrow[row] - Hnextrow[nextrow]); - x = Hrow[nextrow] * Hnextrow[row]; - discriminant = p * p + x; - Hrow[row] += shift; - Hnextrow[nextrow] += shift; - if (discriminant > 0.0) { // pair of real roots - q = sqrt(discriminant); - if (p < 0.0) q = p - q; else q += p; - eigen_real[row] = Hnextrow[nextrow] + q; - eigen_real[nextrow] = Hnextrow[nextrow] - x / q; - eigen_imag[row] = 0.0; - eigen_imag[nextrow] = 0.0; - r = sqrt(Hnextrow[row] * Hnextrow[row] + q * q); - sin = Hnextrow[row] / r; - cos = q / r; - Update_Row(Hrow, cos, sin, n, row); - Update_Column(H, cos, sin, n, row); - Update_Transformation(S, cos, sin, n, row); - } - else { // pair of complex roots - eigen_real[nextrow] = eigen_real[row] = Hnextrow[nextrow] + p; - eigen_imag[row] = sqrt(fabs(discriminant)); - eigen_imag[nextrow] = -eigen_imag[row]; - } -} - - -//////////////////////////////////////////////////////////////////////////////// -// static void Update_Row(double *Hrow, double cos, double sin, int n, // -// int row) // -// // -// Description: // -// Update rows 'row' and 'row + 1' using the rotation matrix: // -// | cos sin | // -// |-sin cos |. // -// I.e. multiply the matrix H on the left by the identity matrix with // -// the 2x2 diagonal block starting at row 'row' replaced by the above // -// 2x2 rotation matrix. // -// // -// Arguments: // -// double Hrow[] // -// Pointer to the row "row" of the matrix in Hessenberg form. // -// double cos // -// Cosine of the rotation angle. // -// double sin // -// Sine of the rotation angle. // -// int n // -// The dimension of the matrix H. // -// int row // -// The row to which the pointer Hrow[] points of the matrix H // -// in Hessenberg form. // -//////////////////////////////////////////////////////////////////////////////// -// // -static void Update_Row(double *Hrow, double cos, double sin, int n, int row) -{ - double x; - double *Hnextrow = Hrow + n; - int i; - - for (i = row; i < n; i++) { - x = Hrow[i]; - Hrow[i] = cos * x + sin * Hnextrow[i]; - Hnextrow[i] = cos * Hnextrow[i] - sin * x; - } -} - - -//////////////////////////////////////////////////////////////////////////////// -// static void Update_Column(double* H, double cos, double sin, int n, // -// int col) // -// // -// Description: // -// Update columns 'col' and 'col + 1' using the rotation matrix: // -// | cos -sin | // -// | sin cos |. // -// I.e. multiply the matrix H on the right by the identity matrix with // -// the 2x2 diagonal block starting at row 'col' replaced by the above // -// 2x2 rotation matrix. // -// // -// Arguments: // -// double *H // -// Pointer to the matrix in Hessenberg form. // -// double cos // -// Cosine of the rotation angle. // -// double sin // -// Sine of the rotation angle. // -// int n // -// The dimension of the matrix H. // -// int col // -// The left-most column of the matrix H to update. // -//////////////////////////////////////////////////////////////////////////////// -// // -static void Update_Column(double* H, double cos, double sin, int n, int col) -{ - double x; - int i; - int next_col = col + 1; - - for (i = 0; i <= next_col; i++, H += n) { - x = H[col]; - H[col] = cos * x + sin * H[next_col]; - H[next_col] = cos * H[next_col] - sin * x; - } -} - - -//////////////////////////////////////////////////////////////////////////////// -// static void Update_Transformation(double *S, double cos, double sin, // -// int n, int k) // -// // -// Description: // -// Update columns 'k' and 'k + 1' using the rotation matrix: // -// | cos -sin | // -// | sin cos |. // -// I.e. multiply the matrix S on the right by the identity matrix with // -// the 2x2 diagonal block starting at row 'k' replaced by the above // -// 2x2 rotation matrix. // -// // -// Arguments: // -// double *S // -// Pointer to the row "row" of the matrix in Hessenberg form. // -// double cos // -// Pointer to the first element of the matrix in Hessenberg form. // -// double sin // -// Pointer to the first element of the transformation matrix. // -// int n // -// The dimensions of the matrix H and S. // -// int k // -// The row to which the pointer Hrow[] points of the matrix H. // -//////////////////////////////////////////////////////////////////////////////// -// // -static void Update_Transformation(double *S, double cos, double sin, - int n, int k) -{ - double x; - int i; - int k1 = k + 1; - - for (i = 0; i < n; i++, S += n) { - x = S[k]; - S[k] = cos * x + sin * S[k1]; - S[k1] = cos * S[k1] - sin * x; - } -} - - -//////////////////////////////////////////////////////////////////////////////// -// static void Double_QR_Iteration(double *H, double *S, int min_row, // -// int max_row, int n, double* shift, int iteration) // -// // -// Description: // -// Calculate the trace and determinant of the 2x2 matrix: // -// | H[k-1][k-1] H[k-1][k] | // -// | H[k][k-1] H[k][k] | // -// unless iteration = 0 (mod 10) in which case increment the shift and // -// decrement the first k elements of the matrix H, then fudge the trace // -// and determinant by trace = 3/2( |H[k][k-1]| + |H[k-1][k-2]| and // -// det = 4/9 trace^2. // -// // -// Arguments: // -// double *H // -// Pointer to the matrix H in Hessenberg form. // -// double *S // -// Pointer to the transformation matrix S. // -// int min_row // -// The top-most row in which the off-diagonal element of H is // -// negligible. If no such row exists, then min_row = 0. // -// int max_row // -// The maximum row of the block 2 x 2 diagonal matrix used to // -// estimate the two eigenvalues for the two implicit shifts. // -// int n // -// The dimensions of the matrix H and S. // -// double *shift // -// The cumulative exceptional shift of the diagonal elements of // -// the matrix H. // -// int iteration // -// Current iteration count. // -//////////////////////////////////////////////////////////////////////////////// -// // -static void Double_QR_Iteration(double *H, double *S, int min_row, int max_row, - int n, double* shift, int iteration) -{ - int k; - double trace, det; - - Product_and_Sum_of_Shifts(H, n, max_row, shift, &trace, &det, iteration); - k = Two_Consecutive_Small_Subdiagonal(H, min_row, max_row, n, trace, det); - Double_QR_Step(H, min_row, max_row, k, trace, det, S, n); -} - - -//////////////////////////////////////////////////////////////////////////////// -// static void Product_and_Sum_of_Shifts(double *H, int n, int max_row, // -// double* shift, double *trace, double *det, int iteration) // -// // -// Description: // -// Calculate the trace and determinant of the 2x2 matrix: // -// | H[k-1][k-1] H[k-1][k] | // -// | H[k][k-1] H[k][k] | // -// unless iteration = 0 (mod 10) in which case increment the shift and // -// decrement the first k elements of the matrix H, then fudge the trace // -// and determinant by trace = 3/2( |H[k][k-1]| + |H[k-1][k-2]| and // -// det = 4/9 trace^2. // -// // -// Arguments: // -// double *H // -// Pointer to the matrix H in Hessenberg form. // -// int n // -// The dimension of the matrix H. // -// int max_row // -// The maximum row of the block 2 x 2 diagonal matrix used to // -// estimate the two eigenvalues for the two implicit shifts. // -// double *shift // -// The cumulative exceptional shift of the diagonal elements of // -// the matrix H. Modified if an exceptional shift occurs. // -// double *trace // -// Returns the trace of the 2 x 2 block diagonal matrix starting // -// at the row/column max_row-1. For an exceptional shift, the // -// trace is set as described above. // -// double *det // -// Returns the determinant of the 2 x 2 block diagonal matrix // -// starting at the row/column max_row-1. For an exceptional shift,// -// the determinant is set as described above. // -// int iteration // -// Current iteration count. // -//////////////////////////////////////////////////////////////////////////////// -// // -static void Product_and_Sum_of_Shifts(double *H, int n, int max_row, - double* shift, double *trace, double *det, int iteration) -{ - double *pH = H + max_row * n; - double *p_aux; - int i; - int min_col = max_row - 1; - - if ((iteration % 10) == 0) { - *shift += pH[max_row]; - for (i = 0, p_aux = H; i <= max_row; p_aux += n, i++) - p_aux[i] -= pH[max_row]; - p_aux = pH - n; - *trace = fabs(pH[min_col]) + fabs(p_aux[min_col - 1]); - *det = *trace * *trace; - *trace *= 1.5; - } - else { - p_aux = pH - n; - *trace = p_aux[min_col] + pH[max_row]; - *det = p_aux[min_col] * pH[max_row] - p_aux[max_row] * pH[min_col]; - } -}; - - -//////////////////////////////////////////////////////////////////////////////// -// static int Two_Consecutive_Small_Subdiagonal(double* H, int min_row, // -// int max_row, int n, double trace, double det) // -// // -// Description: // -// To reduce the amount of computation in Francis' double QR step search // -// for two consecutive small subdiagonal elements from row nn to row m, // -// where m < nn. // -// // -// Arguments: // -// double *H // -// Pointer to the first element of the matrix in Hessenberg form. // -// int min_row // -// The row in which to end the search (search is from upwards). // -// int max_row // -// The row in which to begin the search. // -// int n // -// The dimension of H. // -// double trace // -// The trace of the lower 2 x 2 block diagonal matrix. // -// double det // -// The determinant of the lower 2 x 2 block diagonal matrix. // -// // -// Return Value: // -// Row with negligible subdiagonal element or min_row if none found. // -//////////////////////////////////////////////////////////////////////////////// -// // -static int Two_Consecutive_Small_Subdiagonal(double* H, int min_row, - int max_row, int n, double trace, double det) -{ - double x, y, z, s; - double* pH; - int i, k; - - for (k = max_row - 2, pH = H + k * n; k >= min_row; pH -= n, k--) { - x = (pH[k] * (pH[k] - trace) + det) / pH[n + k] + pH[k + 1]; - y = pH[k] + pH[n + k + 1] - trace; - z = pH[n + n + k + 1]; - s = fabs(x) + fabs(y) + fabs(z); - x /= s; - y /= s; - z /= s; - if (k == min_row) break; - if ((fabs(pH[k - 1]) * (fabs(y) + fabs(z))) <= - DBL_EPSILON * fabs(x) * - (fabs(pH[k - 1 - n]) + fabs(pH[k]) + fabs(pH[n + k + 1]))) break; - } - for (i = k + 2, pH = H + i * n; i <= max_row; pH += n, i++) pH[i - 2] = 0.0; - for (i = k + 3, pH = H + i * n; i <= max_row; pH += n, i++) pH[i - 3] = 0.0; - return k; -}; - - -//////////////////////////////////////////////////////////////////////////////// -// static void Double_QR_Step(double *H, int min_row, int max_row, // -// int min_col, double *S, int n) // -// // -// Description: // -// Perform Francis' double QR step from rows 'min_row' to 'max_row' // -// and columns 'min_col' to 'max_row'. // -// // -// Arguments: // -// double *H // -// Pointer to the first element of the matrix in Hessenberg form. // -// int min_row // -// The row in which to begin. // -// int max_row // -// The row in which to end. // -// int min_col // -// The column in which to begin. // -// double trace // -// The trace of the lower 2 x 2 block diagonal matrix. // -// double det // -// The determinant of the lower 2 x 2 block diagonal matrix. // -// double *S // -// Pointer to the first element of the transformation matrix. // -// int n // -// The dimensions of H and S. // -//////////////////////////////////////////////////////////////////////////////// -// // -static void Double_QR_Step(double *H, int min_row, int max_row, int min_col, - double trace, double det, double *S, int n) -{ - double s, x, y, z; - double a, b, c; - double *pH; - double *tH; - double *pS; - int i, j, k; - int last_test_row_col = max_row - 1; - - k = min_col; - pH = H + min_col * n; - a = (pH[k] * (pH[k] - trace) + det) / pH[n + k] + pH[k + 1]; - b = pH[k] + pH[n + k + 1] - trace; - c = pH[n + n + k + 1]; - s = fabs(a) + fabs(b) + fabs(c); - a /= s; - b /= s; - c /= s; - - for (; k <= last_test_row_col; k++, pH += n) { - if (k > min_col) { - c = (k == last_test_row_col) ? 0.0 : pH[n + n + k - 1]; - x = fabs(pH[k - 1]) + fabs(pH[n + k - 1]) + fabs(c); - if (x == 0.0) continue; - a = pH[k - 1] / x; - b = pH[n + k - 1] / x; - c /= x; - } - s = sqrt(a * a + b * b + c * c); - if (a < 0.0) s = -s; - if (k > min_col) pH[k - 1] = -s * x; - else if (min_row != min_col) pH[k - 1] = -pH[k - 1]; - a += s; - x = a / s; - y = b / s; - z = c / s; - b /= a; - c /= a; - - // Update rows k, k+1, k+2 - for (j = k; j < n; j++) { - a = pH[j] + b * pH[n + j]; - if (k != last_test_row_col) { - a += c * pH[n + n + j]; - pH[n + n + j] -= a * z; - } - pH[n + j] -= a * y; - pH[j] -= a * x; - } - - // Update column k+1 - - j = k + 3; - if (j > max_row) j = max_row; - for (i = 0, tH = H; i <= j; i++, tH += n) { - a = x * tH[k] + y * tH[k + 1]; - if (k != last_test_row_col) { - a += z * tH[k + 2]; - tH[k + 2] -= a * c; - } - tH[k + 1] -= a * b; - tH[k] -= a; - } - - // Update transformation matrix - - for (i = 0, pS = S; i < n; pS += n, i++) { - a = x * pS[k] + y * pS[k + 1]; - if (k != last_test_row_col) { - a += z * pS[k + 2]; - pS[k + 2] -= a * c; - } - pS[k + 1] -= a * b; - pS[k] -= a; - } - }; -} - - -//////////////////////////////////////////////////////////////////////////////// -// static void BackSubstitution(double *H, double eigen_real[], // -// double eigen_imag[], int n) // -// // -// Description: // -// // -// Arguments: // -// double *H // -// Pointer to the first element of the matrix in Hessenberg form. // -// double eigen_real[] // -// The real part of an eigenvalue. // -// double eigen_imag[] // -// The imaginary part of an eigenvalue. // -// int n // -// The dimension of H, eigen_real, and eigen_imag. // -//////////////////////////////////////////////////////////////////////////////// -// // -static void BackSubstitution(double *H, double eigen_real[], - double eigen_imag[], int n) -{ - double zero_tolerance; - double *pH; - int i, j, row; - - // Calculate the zero tolerance - - pH = H; - zero_tolerance = fabs(pH[0]); - for (pH += n, i = 1; i < n; pH += n, i++) - for (j = i - 1; j < n; j++) zero_tolerance += fabs(pH[j]); - zero_tolerance *= DBL_EPSILON; - - // Start Backsubstitution - - for (row = n - 1; row >= 0; row--) { - if (eigen_imag[row] == 0.0) - BackSubstitute_Real_Vector(H, eigen_real, eigen_imag, row, - zero_tolerance, n); - else if (eigen_imag[row] < 0.0) - BackSubstitute_Complex_Vector(H, eigen_real, eigen_imag, row, - zero_tolerance, n); - } -} - - -//////////////////////////////////////////////////////////////////////////////// -// static void BackSubstitute_Real_Vector(double *H, double eigen_real[], // -// double eigen_imag[], int row, double zero_tolerance, int n) // -// // -// Description: // -// // -// Arguments: // -// double *H // -// Pointer to the first element of the matrix in Hessenberg form. // -// double eigen_real[] // -// The real part of an eigenvalue. // -// double eigen_imag[] // -// The imaginary part of an eigenvalue. // -// int row // -// double zero_tolerance // -// Zero substitute. To avoid dividing by zero. // -// int n // -// The dimension of H, eigen_real, and eigen_imag. // -//////////////////////////////////////////////////////////////////////////////// -// // -static void BackSubstitute_Real_Vector(double *H, double eigen_real[], - double eigen_imag[], int row, double zero_tolerance, int n) -{ - double *pH; - double *pV; - double x, y; - double u[4]; - double v[2]; - int i, j, k; - - k = row; - pH = H + row * n; - pH[row] = 1.0; - for (i = row - 1, pH -= n; i >= 0; i--, pH -= n) { - u[0] = pH[i] - eigen_real[row]; - v[0] = pH[row]; - pV = H + n * k; - for (j = k; j < row; j++, pV += n) v[0] += pH[j] * pV[row]; - if (eigen_imag[i] < 0.0) { - u[3] = u[0]; - v[1] = v[0]; - } - else { - k = i; - if (eigen_imag[i] == 0.0) { - if (u[0] != 0.0) pH[row] = -v[0] / u[0]; - else pH[row] = -v[0] / zero_tolerance; - } - else { - u[1] = pH[i + 1]; - u[2] = pH[n + i]; - x = (eigen_real[i] - eigen_real[row]); - x *= x; - x += eigen_imag[i] * eigen_imag[i]; - pH[row] = (u[1] * v[1] - u[3] * v[0]) / x; - if (fabs(u[1]) > fabs(u[3])) - pH[n + row] = -(v[0] + u[0] * pH[row]) / u[1]; - else - pH[n + row] = -(v[1] + u[2] * pH[row]) / u[3]; - } - } - } -} - - -//////////////////////////////////////////////////////////////////////////////// -// static void BackSubstitute_Complex_Vector(double *H, double eigen_real[], // -// double eigen_imag[], int row, double zero_tolerance, int n) // -// // -// Description: // -// // -// Arguments: // -// double *H // -// Pointer to the first element of the matrix in Hessenberg form. // -// double eigen_real[] // -// The real part of an eigenvalue. // -// double eigen_imag[] // -// The imaginary part of an eigenvalue. // -// int row // -// double zero_tolerance // -// Zero substitute. To avoid dividing by zero. // -// int n // -// The dimension of H, eigen_real, and eigen_imag. // -//////////////////////////////////////////////////////////////////////////////// -// // -static void BackSubstitute_Complex_Vector(double *H, double eigen_real[], - double eigen_imag[], int row, double zero_tolerance, int n) -{ - double *pH; - double *pV; - double x, y; - double u[4]; - double v[2]; - double w[2]; - int i, j, k; - - k = row - 1; - pH = H + n * row; - if (fabs(pH[k]) > fabs(pH[row - n])) { - pH[k - n] = -(pH[row] - eigen_real[row]) / pH[k]; - pH[row - n] = -eigen_imag[row] / pH[k]; - } - else - Complex_Division(-pH[row - n], 0.0, - pH[k - n] - eigen_real[row], eigen_imag[row], &pH[k - n], &pH[row - n]); - pH[k] = 1.0; - pH[row] = 0.0; - for (i = row - 2, pH = H + n * i; i >= 0; pH -= n, i--) { - u[0] = pH[i] - eigen_real[row]; - w[0] = pH[row]; - w[1] = 0.0; - pV = H + k * n; - for (j = k; j < row; j++, pV += n) { - w[0] += pH[j] * pV[row - 1]; - w[1] += pH[j] * pV[row]; - } - if (eigen_imag[i] < 0.0) { - u[3] = u[0]; - v[0] = w[0]; - v[1] = w[1]; - } - else { - k = i; - if (eigen_imag[i] == 0.0) { - Complex_Division(-w[0], -w[1], u[0], eigen_imag[row], &pH[row - 1], - &pH[row]); - } - else { - u[1] = pH[i + 1]; - u[2] = pH[n + i]; - x = eigen_real[i] - eigen_real[row]; - y = 2.0 * x * eigen_imag[row]; - x = x * x + eigen_imag[i] * eigen_imag[i] - - eigen_imag[row] * eigen_imag[row]; - if (x == 0.0 && y == 0.0) - x = zero_tolerance * (fabs(u[0]) + fabs(u[1]) + fabs(u[2]) - + fabs(u[3]) + fabs(eigen_imag[row])); - Complex_Division(u[1] * v[0] - u[3] * w[0] + w[1] * eigen_imag[row], - u[1] * v[1] - u[3] * w[1] - w[0] * eigen_imag[row], - x, y, &pH[row - 1], &pH[row]); - if (fabs(u[1]) > (fabs(u[3]) + fabs(eigen_imag[row]))) { - pH[n + row - 1] = -w[0] - u[0] * pH[row - 1] - + eigen_imag[row] * pH[row] / u[1]; - pH[n + row] = -w[1] - u[0] * pH[row] - - eigen_imag[row] * pH[row - 1] / u[1]; - } - else { - Complex_Division(-v[0] - u[2] * pH[row - 1], -v[1] - u[2] * pH[row], - u[3], eigen_imag[row], &pH[n + row - 1], &pH[n + row]); - } - } - } - } -} - -//////////////////////////////////////////////////////////////////////////////// -// static void Calculate_Eigenvectors(double *H, double *S, // -// double eigen_real[], double eigen_imag[], int n) // -// // -// Description: // -// Multiply by transformation matrix. // -// // -// Arguments: // -// double *H // -// Pointer to the first element of the matrix in Hessenberg form. // -// double *S // -// Pointer to the first element of the transformation matrix. // -// double eigen_real[] // -// The real part of an eigenvalue. // -// double eigen_imag[] // -// The imaginary part of an eigenvalue. // -// int n // -// The dimension of H, S, eigen_real, and eigen_imag. // -//////////////////////////////////////////////////////////////////////////////// -// // -static void Calculate_Eigenvectors(double *H, double *S, double eigen_real[], - double eigen_imag[], int n) -{ - double* pH; - double* pS; - double x, y; - int i, j, k; - - for (k = n - 1; k >= 0; k--) { - if (eigen_imag[k] < 0.0) { - for (i = 0, pS = S; i < n; pS += n, i++) { - x = 0.0; - y = 0.0; - for (j = 0, pH = H; j <= k; pH += n, j++) { - x += pS[j] * pH[k - 1]; - y += pS[j] * pH[k]; - } - pS[k - 1] = x; - pS[k] = y; - } - } - else if (eigen_imag[k] == 0.0) { - for (i = 0, pS = S; i < n; i++, pS += n) { - x = 0.0; - for (j = 0, pH = H; j <= k; j++, pH += n) - x += pS[j] * pH[k]; - pS[k] = x; - } - } - } -} - - -//////////////////////////////////////////////////////////////////////////////// -// static void Complex_Division(double x, double y, double u, double v, // -// double* a, double* b) // -// // -// Description: // -// a + i b = (x + iy) / (u + iv) // -// = (x * u + y * v) / r^2 + i (y * u - x * v) / r^2, // -// where r^2 = u^2 + v^2. // -// // -// Arguments: // -// double x // -// Real part of the numerator. // -// double y // -// Imaginary part of the numerator. // -// double u // -// Real part of the denominator. // -// double v // -// Imaginary part of the denominator. // -// double *a // -// Real part of the quotient. // -// double *b // -// Imaginary part of the quotient. // -//////////////////////////////////////////////////////////////////////////////// -// // -static void Complex_Division(double x, double y, double u, double v, - double* a, double* b) -{ - double q = u*u + v*v; - - *a = (x * u + y * v) / q; - *b = (y * u - x * v) / q; -} -//////////////////////////////////////////////////////////////////////////////// -// File: transpose_square_matrix.c // -// Routine(s): // -// Transpose_Square_Matrix // -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -// void Transpose_Square_Matrix( double *A, int n ) // -// // -// Description: // -// Take the transpose of A and store in place. // -// // -// Arguments: // -// double *A Pointer to the first element of the matrix A. // -// int n The number of rows and columns of the matrix A. // -// // -// Return Values: // -// void // -// // -// Example: // -// #define N // -// double A[N][N]; // -// // -// (your code to initialize the matrix A) // -// // -// Transpose_Square_Matrix( &A[0][0], N); // -// printf("The transpose of A is \n"); ... // -//////////////////////////////////////////////////////////////////////////////// -void Transpose_Square_Matrix(double *A, int n) -{ - double *pA, *pAt; - double temp; - int i, j; - - for (i = 0; i < n; A += n + 1, i++) { - pA = A + 1; - pAt = A + n; - for (j = i + 1; j < n; pA++, pAt += n, j++) { - temp = *pAt; - *pAt = *pA; - *pA = temp; - } - } -} -/////////////////////////////////////////////////////////////////////////////// -// File: lower_triangular.c // -// Routines: // -// Lower_Triangular_Solve // -// Lower_Triangular_Inverse // -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -// int Lower_Triangular_Solve(double *L, double *B, double x[], int n) // -// // -// Description: // -// This routine solves the linear equation Lx = B, where L is an n x n // -// lower triangular matrix. (The superdiagonal part of the matrix is // -// not addressed.) // -// The algorithm follows: // -// x[0] = B[0]/L[0][0], and // -// x[i] = [B[i] - (L[i][0] * x[0] + ... + L[i][i-1] * x[i-1])] / L[i][i],// -// for i = 1, ..., n-1. // -// // -// Arguments: // -// double *L Pointer to the first element of the lower triangular // -// matrix. // -// double *B Pointer to the column vector, (n x 1) matrix, B. // -// double *x Pointer to the column vector, (n x 1) matrix, x. // -// int n The number of rows or columns of the matrix L. // -// // -// Return Values: // -// 0 Success // -// -1 Failure - The matrix L is singular. // -// // -// Example: // -// #define N // -// double A[N][N], B[N], x[N]; // -// // -// (your code to create matrix A and column vector B) // -// err = Lower_Triangular_Solve(&A[0][0], B, x, n); // -// if (err < 0) printf(" Matrix A is singular\n"); // -// else printf(" The solution is \n"); // -// ... // -//////////////////////////////////////////////////////////////////////////////// -// // -int Lower_Triangular_Solve(double *L, double B[], double x[], int n) -{ - int i, k; - - // Solve the linear equation Lx = B for x, where L is a lower - // triangular matrix. - - for (k = 0; k < n; L += n, k++) { - if (*(L + k) == 0.0) return -1; // The matrix L is singular - x[k] = B[k]; - for (i = 0; i < k; i++) x[k] -= x[i] * *(L + i); - x[k] /= *(L + k); - } - - return 0; -} - - -//////////////////////////////////////////////////////////////////////////////// -// int Lower_Triangular_Inverse(double *L, int n) // -// // -// Description: // -// This routine calculates the inverse of the lower triangular matrix L. // -// The superdiagonal part of the matrix is not addressed. // -// The algorithm follows: // -// Let M be the inverse of L, then L M = I, // -// M[i][i] = 1.0 / L[i][i] for i = 0, ..., n-1, and // -// M[i][j] = -[(L[i][j] M[j][j] + ... + L[i][i-1] M[i-1][j])] / L[i][i], // -// for i = 1, ..., n-1, j = 0, ..., i - 1. // -// // -// // -// Arguments: // -// double *L On input, the pointer to the first element of the matrix // -// whose lower triangular elements form the matrix which is // -// to be inverted. On output, the lower triangular part is // -// replaced by the inverse. The superdiagonal elements are // -// not modified. // -// its inverse. // -// int n The number of rows and/or columns of the matrix L. // -// // -// Return Values: // -// 0 Success // -// -1 Failure - The matrix L is singular. // -// // -// Example: // -// #define N // -// double L[N][N]; // -// // -// (your code to create the matrix L) // -// err = Lower_Triangular_Inverse(&L[0][0], N); // -// if (err < 0) printf(" Matrix L is singular\n"); // -// else { // -// printf(" The inverse is \n"); // -// ... // -// } // -//////////////////////////////////////////////////////////////////////////////// -// // -int Lower_Triangular_Inverse(double *L, int n) -{ - int i, j, k; - double *p_i, *p_j, *p_k; - double sum; - - // Invert the diagonal elements of the lower triangular matrix L. - - for (k = 0, p_k = L; k < n; p_k += (n + 1), k++) { - if (*p_k == 0.0) return -1; - else *p_k = 1.0 / *p_k; - } - - // Invert the remaining lower triangular matrix L row by row. - - for (i = 1, p_i = L + n; i < n; i++, p_i += n) { - for (j = 0, p_j = L; j < i; p_j += n, j++) { - sum = 0.0; - for (k = j, p_k = p_j; k < i; k++, p_k += n) - sum += *(p_i + k) * *(p_k + j); - *(p_i + j) = -*(p_i + i) * sum; - } - } - - return 0; -} -//////////////////////////////////////////////////////////////////////////////// -// File: upper_triangular.c // -// Routines: // -// Upper_Triangular_Solve // -// Upper_Triangular_Inverse // -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -// int Upper_Triangular_Solve(double *U, double *B, double x[], int n) // -// // -// Description: // -// This routine solves the linear equation Ux = B, where U is an n x n // -// upper triangular matrix. (The subdiagonal part of the matrix is // -// not addressed.) // -// The algorithm follows: // -// x[n-1] = B[n-1]/U[n-1][n-1], and // -// x[i] = [B[i] - (U[i][i+1] * x[i+1] + ... + U[i][n-1] * x[n-1])] // -// / U[i][i], // -// for i = n-2, ..., 0. // -// // -// Arguments: // -// double *U Pointer to the first element of the upper triangular // -// matrix. // -// double *B Pointer to the column vector, (n x 1) matrix, B. // -// double *x Pointer to the column vector, (n x 1) matrix, x. // -// int n The number of rows or columns of the matrix U. // -// // -// Return Values: // -// 0 Success // -// -1 Failure - The matrix U is singular. // -// // -// Example: // -// #define N // -// double A[N][N], B[N], x[N]; // -// // -// (your code to create matrix A and column vector B) // -// err = Upper_Triangular_Solve(&A[0][0], B, x, n); // -// if (err < 0) printf(" Matrix A is singular\n"); // -// else printf(" The solution is \n"); // -// ... // -//////////////////////////////////////////////////////////////////////////////// -// // -int Upper_Triangular_Solve(double *U, double B[], double x[], int n) -{ - int i, k; - - // Solve the linear equation Ux = B for x, where U is an upper - // triangular matrix. - - for (k = n - 1, U += n * (n - 1); k >= 0; U -= n, k--) { - if (*(U + k) == 0.0) return -1; // The matrix U is singular - x[k] = B[k]; - for (i = k + 1; i < n; i++) x[k] -= x[i] * *(U + i); - x[k] /= *(U + k); - } - - return 0; -} - - -//////////////////////////////////////////////////////////////////////////////// -// int Upper_Triangular_Inverse(double *U, int n) // -// // -// Description: // -// This routine calculates the inverse of the upper triangular matrix U. // -// The subdiagonal part of the matrix is not addressed. // -// The algorithm follows: // -// Let M be the inverse of U, then U M = I, // -// M[n-1][n-1] = 1.0 / U[n-1][n-1] and // -// M[i][j] = -( U[i][i+1] M[i+1][j] + ... + U[i][j] M[j][j] ) / U[i][i], // -// for i = n-2, ... , 0, j = n-1, ..., i+1. // -// // -// // -// Arguments: // -// double *U On input, the pointer to the first element of the matrix // -// whose upper triangular elements form the matrix which is // -// to be inverted. On output, the upper triangular part is // -// replaced by the inverse. The subdiagonal elements are // -// not modified. // -// int n The number of rows and/or columns of the matrix U. // -// // -// Return Values: // -// 0 Success // -// -1 Failure - The matrix U is singular. // -// // -// Example: // -// #define N // -// double U[N][N]; // -// // -// (your code to create the matrix U) // -// err = Upper_Triangular_Inverse(&U[0][0], N); // -// if (err < 0) printf(" Matrix U is singular\n"); // -// else { // -// printf(" The inverse is \n"); // -// ... // -// } // -//////////////////////////////////////////////////////////////////////////////// -// // -int Upper_Triangular_Inverse(double *U, int n) -{ - int i, j, k; - double *p_i, *p_j, *p_k; - double sum; - - // Invert the diagonal elements of the upper triangular matrix U. - - for (k = 0, p_k = U; k < n; p_k += (n + 1), k++) { - if (*p_k == 0.0) return -1; - else *p_k = 1.0 / *p_k; - } - - // Invert the remaining upper triangular matrix U. - - for (i = n - 2, p_i = U + n * (n - 2); i >= 0; p_i -= n, i--) { - for (j = n - 1; j > i; j--) { - sum = 0.0; - for (k = i + 1, p_k = p_i + n; k <= j; p_k += n, k++) { - sum += *(p_i + k) * *(p_k + j); - } - *(p_i + j) = -*(p_i + i) * sum; - } - } - - return 0; -} -//////////////////////////////////////////////////////////////////////////////// -// File: interchange_cols.c // -// Routine(s): // -// Interchange_Columns // -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -// void Interchange_Columns(double *A, int col1, int col2, int nrows, // -// int ncols) // -// // -// Description: // -// Interchange the columns 'col1' and 'col2' of the nrows x ncols // -// matrix A. // -// // -// Arguments: // -// double *A Pointer to the first element of the matrix A. // -// int col1 The column of A which is to be interchanged with col2. // -// int col2 The column of A which is to be interchanged with col1. // -// int nrows The number of rows matrix A. // -// int ncols The number of columns of the matrix A. // -// // -// Return Values: // -// void // -// // -// Example: // -// #define N // -// #define M // -// double A[M][N]; // -// int i,j; // -// // -// (your code to initialize the matrix A, the column number i and column // -// number j) // -// // -// if ( (i >= 0) && ( i < N ) && ( j >= 0 ) && (j < N) ) // -// Interchange_Columns(&A[0][0], i, j, M, N); // -// printf("The matrix A is \n"); ... // -//////////////////////////////////////////////////////////////////////////////// -void Interchange_Columns(double *A, int col1, int col2, int nrows, int ncols) -{ - int i; - double *pA1, *pA2; - double temp; - - pA1 = A + col1; - pA2 = A + col2; - for (i = 0; i < nrows; pA1 += ncols, pA2 += ncols, i++) { - temp = *pA1; - *pA1 = *pA2; - *pA2 = temp; - } -} -//////////////////////////////////////////////////////////////////////////////// -// File: interchange_rows.c // -// Routine(s): // -// Interchange_Rows // -//////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -// void Interchange_Rows(double *A, int row1, int row2, int ncols) // -// // -// Description: // -// Interchange the rows 'row1' and 'row2' of the nrows x ncols matrix A. // -// // -// Arguments: // -// double *A Pointer to the first element of the matrix A. // -// int row1 The row of A which is to be interchanged with row row2. // -// int row2 The row of A which is to be interchanged with row row1. // -// int ncols The number of columns of the matrix A. // -// // -// Return Values: // -// void // -// // -// Example: // -// #define N // -// #define M // -// double A[M][N]; // -// int i, j; // -// // -// (your code to initialize the matrix A, the row number i and row number j) // -// // -// if ( (i >= 0) && ( i < M ) && (j > 0) && ( j < M ) ) // -// Interchange_Rows(&A[0][0], i, j, N); // -// printf("The matrix A is \n"); ... // -//////////////////////////////////////////////////////////////////////////////// -void Interchange_Rows(double *A, int row1, int row2, int ncols) -{ - int i; - double *pA1, *pA2; - double temp; - - pA1 = A + row1 * ncols; - pA2 = A + row2 * ncols; - for (i = 0; i < ncols; i++) { - temp = *pA1; - *pA1++ = *pA2; - *pA2++ = temp; - } -} diff --git a/native/MagnetoLib/MagnetoLib.cpp b/native/MagnetoLib/MagnetoLib.cpp deleted file mode 100644 index 67960bcde..000000000 --- a/native/MagnetoLib/MagnetoLib.cpp +++ /dev/null @@ -1,355 +0,0 @@ -// This is the main DLL file. - -#include "stdafx.h" -#include "magnetolib.h" -#include "magneto.h" - -double calculateHnorm(double data[], int nlines) { - double x, y, z, x2, xave; - int i; - // - // calculate mean (norm) and standard deviation for possible outlier rejection - // - - xave = 0; - for (i = 0; i < nlines; i++) - { - x = data[i * 3 + 0]; - y = data[i * 3 + 1]; - z = data[i * 3 + 2]; - x2 = x*x + y*y + z*z; - xave += sqrt(x2); - } - xave = xave / nlines; //mean vector length - - return xave; -} - -void calculate(double data[], int nlines, double nxsrej, double hm, double B[3], double A_1[3 * 3]) -{ - int nlines2 = 0; - char buf[120]; - double *D, *S, *C, *S11, *S12, *S12t, *S22, *S22_1, *S22a, *S22b, *SS, *E, *d, *U, *SSS; - double *eigen_real, *eigen_imag, *v1, *v2, *v, *Q, *Q_1, *QB, J, hmb, *SSSS; - int *p; - int i, j, index; - double maxval, norm, btqb, *eigen_real3, *eigen_imag3, *Dz, *vdz, *SQ, norm1, norm2, norm3; - double x, y, z, x2, xs, xave; - double *raw; //raw obs - char filename[64]; - - // - // calculate mean (norm) and standard deviation for possible outlier rejection - // - - xs = 0; - xave = 0; - for (i = 0; i < nlines; i++) - { - x = data[i * 3 + 0]; - y = data[i * 3 + 1]; - z = data[i * 3 + 2]; - x2 = x*x + y*y + z*z; - xs += x2; - xave += sqrt(x2); - } - xave = xave / nlines; //mean vector length - xs = sqrt(xs / nlines - (xave*xave)); //std. dev. - - // summarize statistics, give opportunity to reject outlier measurements - - //printf("verage magnitude (sigma) of %d vectors (default Hm) = %6.1lf (%6.1lf)\r\n", nlines, xave, xs); - //printf("Rejection level selected: %lf\r\n ", nxsrej); - - // scan file again - - nlines2 = 0; //count non-rejected - if (nxsrej > 0) { - //printf("\r\nRejecting measurements if abs(vector_length-average)/(std. dev.) > %5.1lf", nxsrej); - - // outlier rejection, count them - for (i = 0; i < nlines; i++) - { - x = data[i * 3 + 0]; - y = data[i * 3 + 1]; - z = data[i * 3 + 2]; - x2 = sqrt(x*x + y*y + z*z); //vector length - x2 = fabs(x2 - xave) / xs; //standard deviations from mean - if (x2 < nxsrej) nlines2++; - } - //printf("\r\n%3d measurements will be rejected", nlines - nlines2); - } - - // third time through! - - if (nlines2 == 0) nlines2 = nlines; //none rejected - j = 0; //index for good measurements - - // allocate array space for accepted measurements - D = (double*)malloc(10 * nlines2 * sizeof(double)); - raw = (double*)malloc(3 * nlines2 * sizeof(double)); - - for (i = 0; i < nlines; i++) - { - x = data[i * 3 + 0]; - y = data[i * 3 + 1]; - z = data[i * 3 + 2]; - x2 = sqrt(x*x + y*y + z*z); //vector length - x2 = fabs(x2 - xave) / xs; //standard deviations from mean - if ((nxsrej == 0) || (x2 < nxsrej)) { - - raw[3 * j] = x; - raw[3 * j + 1] = y; - raw[3 * j + 2] = z; - D[j] = x * x; - D[nlines2 + j] = y * y; - D[nlines2 * 2 + j] = z * z; - D[nlines2 * 3 + j] = 2.0 * y * z; - D[nlines2 * 4 + j] = 2.0 * x * z; - D[nlines2 * 5 + j] = 2.0 * x * y; - D[nlines2 * 6 + j] = 2.0 * x; - D[nlines2 * 7 + j] = 2.0 * y; - D[nlines2 * 8 + j] = 2.0 * z; - D[nlines2 * 9 + j] = 1.0; - j++; //index good measurements - } - } - //printf("\r\n%3d measurements processed, expected %d", j, nlines2); - nlines = nlines2; //reset number of measurements - - if (hm == 0.0) hm = xave; - - // allocate memory for matrix S - S = (double*)malloc(10 * 10 * sizeof(double)); - Multiply_Self_Transpose(S, D, 10, nlines); - - // Create pre-inverted constraint matrix C - C = (double*)malloc(6 * 6 * sizeof(double)); - C[0] = 0.0; C[1] = 0.5; C[2] = 0.5; C[3] = 0.0; C[4] = 0.0; C[5] = 0.0; - C[6] = 0.5; C[7] = 0.0; C[8] = 0.5; C[9] = 0.0; C[10] = 0.0; C[11] = 0.0; - C[12] = 0.5; C[13] = 0.5; C[14] = 0.0; C[15] = 0.0; C[16] = 0.0; C[17] = 0.0; - C[18] = 0.0; C[19] = 0.0; C[20] = 0.0; C[21] = -0.25; C[22] = 0.0; C[23] = 0.0; - C[24] = 0.0; C[25] = 0.0; C[26] = 0.0; C[27] = 0.0; C[28] = -0.25; C[29] = 0.0; - C[30] = 0.0; C[31] = 0.0; C[32] = 0.0; C[33] = 0.0; C[34] = 0.0; C[35] = -0.25; - - S11 = (double*)malloc(6 * 6 * sizeof(double)); - Get_Submatrix(S11, 6, 6, S, 10, 0, 0); - S12 = (double*)malloc(6 * 4 * sizeof(double)); - Get_Submatrix(S12, 6, 4, S, 10, 0, 6); - S12t = (double*)malloc(4 * 6 * sizeof(double)); - Get_Submatrix(S12t, 4, 6, S, 10, 6, 0); - S22 = (double*)malloc(4 * 4 * sizeof(double)); - Get_Submatrix(S22, 4, 4, S, 10, 6, 6); - - S22_1 = (double*)malloc(4 * 4 * sizeof(double)); - for (i = 0; i < 16; i++) - S22_1[i] = S22[i]; - Choleski_LU_Decomposition(S22_1, 4); - Choleski_LU_Inverse(S22_1, 4); - - // Calculate S22a = S22_1 * S12t 4*6 = 4x4 * 4x6 C = AB - S22a = (double*)malloc(4 * 6 * sizeof(double)); - Multiply_Matrices(S22a, S22_1, 4, 4, S12t, 6); - - // Then calculate S22b = S12 * S22a ( 6x6 = 6x4 * 4x6) - S22b = (double*)malloc(6 * 6 * sizeof(double)); - Multiply_Matrices(S22b, S12, 6, 4, S22a, 6); - - // Calculate SS = S11 - S22b - SS = (double*)malloc(6 * 6 * sizeof(double)); - for (i = 0; i < 36; i++) - SS[i] = S11[i] - S22b[i]; - E = (double*)malloc(6 * 6 * sizeof(double)); - Multiply_Matrices(E, C, 6, 6, SS, 6); - - SSS = (double*)malloc(6 * 6 * sizeof(double)); - Hessenberg_Form_Elementary(E, SSS, 6); - - eigen_real = (double*)malloc(6 * sizeof(double)); - eigen_imag = (double*)malloc(6 * sizeof(double)); - - QR_Hessenberg_Matrix(E, SSS, eigen_real, eigen_imag, 6, 100); - - index = 0; - maxval = eigen_real[0]; - for (i = 1; i < 6; i++) - { - if (eigen_real[i] > maxval) - { - maxval = eigen_real[i]; - index = i; - } - } - - v1 = (double*)malloc(6 * sizeof(double)); - - v1[0] = SSS[index]; - v1[1] = SSS[index + 6]; - v1[2] = SSS[index + 12]; - v1[3] = SSS[index + 18]; - v1[4] = SSS[index + 24]; - v1[5] = SSS[index + 30]; - - // normalize v1 - norm = sqrt(v1[0] * v1[0] + v1[1] * v1[1] + v1[2] * v1[2] + v1[3] * v1[3] + v1[4] * v1[4] + v1[5] * v1[5]); - v1[0] /= norm; - v1[1] /= norm; - v1[2] /= norm; - v1[3] /= norm; - v1[4] /= norm; - v1[5] /= norm; - - if (v1[0] < 0.0) - { - v1[0] = -v1[0]; - v1[1] = -v1[1]; - v1[2] = -v1[2]; - v1[3] = -v1[3]; - v1[4] = -v1[4]; - v1[5] = -v1[5]; - } - - // Calculate v2 = S22a * v1 ( 4x1 = 4x6 * 6x1) - v2 = (double*)malloc(4 * sizeof(double)); - Multiply_Matrices(v2, S22a, 4, 6, v1, 1); - - v = (double*)malloc(10 * sizeof(double)); - - v[0] = v1[0]; - v[1] = v1[1]; - v[2] = v1[2]; - v[3] = v1[3]; - v[4] = v1[4]; - v[5] = v1[5]; - v[6] = -v2[0]; - v[7] = -v2[1]; - v[8] = -v2[2]; - v[9] = -v2[3]; - - Q = (double*)malloc(3 * 3 * sizeof(double)); - - Q[0] = v[0]; - Q[1] = v[5]; - Q[2] = v[4]; - Q[3] = v[5]; - Q[4] = v[1]; - Q[5] = v[3]; - Q[6] = v[4]; - Q[7] = v[3]; - Q[8] = v[2]; - - U = (double*)malloc(3 * sizeof(double)); - - U[0] = v[6]; - U[1] = v[7]; - U[2] = v[8]; - Q_1 = (double*)malloc(3 * 3 * sizeof(double)); - for (i = 0; i < 9; i++) - Q_1[i] = Q[i]; - Choleski_LU_Decomposition(Q_1, 3); - Choleski_LU_Inverse(Q_1, 3); - - // Calculate B = Q-1 * U ( 3x1 = 3x3 * 3x1) - Multiply_Matrices(B, Q_1, 3, 3, U, 1); - B[0] = -B[0]; // x-axis combined bias - B[1] = -B[1]; // y-axis combined bias - B[2] = -B[2]; // z-axis combined bias - - // for(i = 0; i < 3; i++) - //printf("\r\nCombined bias vector B:\r\n"); - //printf("%8.2lf %8.2lf %8.2lf \r\n", B[0], B[1], B[2]); - - // First calculate QB = Q * B ( 3x1 = 3x3 * 3x1) - QB = (double*)malloc(3 * sizeof(double)); - Multiply_Matrices(QB, Q, 3, 3, B, 1); - - // Then calculate btqb = BT * QB ( 1x1 = 1x3 * 3x1) - Multiply_Matrices(&btqb, B, 1, 3, QB, 1); - - // Calculate hmb = sqrt(btqb - J). - J = v[9]; - hmb = sqrt(btqb - J); - - // Calculate SQ, the square root of matrix Q - SSSS = (double*)malloc(3 * 3 * sizeof(double)); - Hessenberg_Form_Elementary(Q, SSSS, 3); - - eigen_real3 = (double*)malloc(3 * sizeof(double)); - eigen_imag3 = (double*)malloc(3 * sizeof(double)); - QR_Hessenberg_Matrix(Q, SSSS, eigen_real3, eigen_imag3, 3, 100); - - // normalize eigenvectors - norm1 = sqrt(SSSS[0] * SSSS[0] + SSSS[3] * SSSS[3] + SSSS[6] * SSSS[6]); - SSSS[0] /= norm1; - SSSS[3] /= norm1; - SSSS[6] /= norm1; - norm2 = sqrt(SSSS[1] * SSSS[1] + SSSS[4] * SSSS[4] + SSSS[7] * SSSS[7]); - SSSS[1] /= norm2; - SSSS[4] /= norm2; - SSSS[7] /= norm2; - norm3 = sqrt(SSSS[2] * SSSS[2] + SSSS[5] * SSSS[5] + SSSS[8] * SSSS[8]); - SSSS[2] /= norm3; - SSSS[5] /= norm3; - SSSS[8] /= norm3; - - Dz = (double*)malloc(3 * 3 * sizeof(double)); - for (i = 0; i < 9; i++) - Dz[i] = 0.0; - Dz[0] = sqrt(eigen_real3[0]); - Dz[4] = sqrt(eigen_real3[1]); - Dz[8] = sqrt(eigen_real3[2]); - - vdz = (double*)malloc(3 * 3 * sizeof(double)); - Multiply_Matrices(vdz, SSSS, 3, 3, Dz, 3); - - Transpose_Square_Matrix(SSSS, 3); - - SQ = (double*)malloc(3 * 3 * sizeof(double)); - Multiply_Matrices(SQ, vdz, 3, 3, SSSS, 3); - - // hm = 0.569; - for (i = 0; i < 9; i++) - A_1[i] = SQ[i] * hm / hmb; - - /* - printf("\r\nCorrection matrix Ainv, using Hm=%lf:\r\n", hm); - for (i = 0; i < 3; i++) - printf("%9.5lf %9.5lf %9.5lf\r\n", A_1[i * 3], A_1[i * 3 + 1], A_1[i * 3 + 2]); - printf("\r\nWhere Hcalc = Ainv*(H-B)\r\n"); - - printf("\r\nCode initialization statements...\r\n"); - printf("\r\n float B[3]\r\n {%8.2lf,%8.2lf,%8.2lf};\r\n", B[0], B[1], B[2]); - printf("\r\n float Ainv[3][3]\r\n {{%9.5lf,%9.5lf,%9.5lf},\r\n", A_1[0], A_1[1], A_1[2]); - printf(" {%9.5lf,%9.5lf,%9.5lf},\r\n", A_1[3], A_1[4], A_1[5]); - printf(" {%9.5lf,%9.5lf,%9.5lf}};\r\n", A_1[6], A_1[7], A_1[8]); - //*/ - - free(D); - free(S); - free(C); - free(S11); - free(S12); - free(S12t); - free(S22); - free(S22_1); - free(S22a); - free(S22b); - free(SS); - free(E); - free(U); - free(SSS); - free(eigen_real); - free(eigen_imag); - free(v1); - free(v2); - free(v); - free(Q); - free(Q_1); - free(QB); - free(SSSS); - free(eigen_real3); - free(eigen_imag3); - free(Dz); - free(vdz); - free(SQ); -} diff --git a/native/MagnetoLib/MagnetoLib.sln b/native/MagnetoLib/MagnetoLib.sln deleted file mode 100644 index b38bc612a..000000000 --- a/native/MagnetoLib/MagnetoLib.sln +++ /dev/null @@ -1,31 +0,0 @@ - -Microsoft Visual Studio Solution File, Format Version 12.00 -# Visual Studio 15 -VisualStudioVersion = 15.0.26730.15 -MinimumVisualStudioVersion = 10.0.40219.1 -Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "MagnetoLib", "MagnetoLib.vcxproj", "{E9872CD1-7CFB-4344-B141-8583E3A52C34}" -EndProject -Global - GlobalSection(SolutionConfigurationPlatforms) = preSolution - Debug|x64 = Debug|x64 - Debug|x86 = Debug|x86 - Release|x64 = Release|x64 - Release|x86 = Release|x86 - EndGlobalSection - GlobalSection(ProjectConfigurationPlatforms) = postSolution - {E9872CD1-7CFB-4344-B141-8583E3A52C34}.Debug|x64.ActiveCfg = Debug|x64 - {E9872CD1-7CFB-4344-B141-8583E3A52C34}.Debug|x64.Build.0 = Debug|x64 - {E9872CD1-7CFB-4344-B141-8583E3A52C34}.Debug|x86.ActiveCfg = Debug|Win32 - {E9872CD1-7CFB-4344-B141-8583E3A52C34}.Debug|x86.Build.0 = Debug|Win32 - {E9872CD1-7CFB-4344-B141-8583E3A52C34}.Release|x64.ActiveCfg = Release|x64 - {E9872CD1-7CFB-4344-B141-8583E3A52C34}.Release|x64.Build.0 = Release|x64 - {E9872CD1-7CFB-4344-B141-8583E3A52C34}.Release|x86.ActiveCfg = Release|Win32 - {E9872CD1-7CFB-4344-B141-8583E3A52C34}.Release|x86.Build.0 = Release|Win32 - EndGlobalSection - GlobalSection(SolutionProperties) = preSolution - HideSolutionNode = FALSE - EndGlobalSection - GlobalSection(ExtensibilityGlobals) = postSolution - SolutionGuid = {3F5C7B4A-6B24-4FE5-A65E-8CCED5E98FD6} - EndGlobalSection -EndGlobal diff --git a/native/MagnetoLib/MagnetoLib.vcxproj b/native/MagnetoLib/MagnetoLib.vcxproj deleted file mode 100644 index d71310779..000000000 --- a/native/MagnetoLib/MagnetoLib.vcxproj +++ /dev/null @@ -1,164 +0,0 @@ - - - - - Debug - Win32 - - - Release - Win32 - - - Debug - x64 - - - Release - x64 - - - - 15.0 - {E9872CD1-7CFB-4344-B141-8583E3A52C34} - v4.6.1 - ManagedCProj - MagnetoLib - 10.0.19041.0 - - - - DynamicLibrary - true - v141 - true - Unicode - - - DynamicLibrary - false - v141 - true - Unicode - - - DynamicLibrary - true - v141 - true - Unicode - - - DynamicLibrary - false - v141 - true - Unicode - - - - - - - - - - - - - - - - - - - - - true - - - true - - - false - - - false - - - - Level3 - Disabled - WIN32;_DEBUG;%(PreprocessorDefinitions) - Use - - - - - - - - Level3 - Disabled - _DEBUG;%(PreprocessorDefinitions) - Use - - - - - - - - Level3 - WIN32;NDEBUG;%(PreprocessorDefinitions) - Use - - - - - - - - Level3 - NDEBUG;%(PreprocessorDefinitions) - Use - - - - - - - - - - - - - - - - - - - - - - Create - Create - Create - Create - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/native/MagnetoLib/MagnetoLib.vcxproj.filters b/native/MagnetoLib/MagnetoLib.vcxproj.filters deleted file mode 100644 index 496453120..000000000 --- a/native/MagnetoLib/MagnetoLib.vcxproj.filters +++ /dev/null @@ -1,58 +0,0 @@ - - - - - {4FC737F1-C7A5-4376-A066-2A32D752A2FF} - cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx - - - {93995380-89BD-4b04-88EB-625FBE52EBFB} - h;hh;hpp;hxx;hm;inl;inc;xsd - - - {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} - rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms - - - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - - - - - - Resource Files - - - - - Resource Files - - - \ No newline at end of file diff --git a/native/MagnetoLib/app.rc b/native/MagnetoLib/app.rc deleted file mode 100644 index 29f148e16..000000000 Binary files a/native/MagnetoLib/app.rc and /dev/null differ diff --git a/native/MagnetoLib/assemblyinfo.cpp b/native/MagnetoLib/assemblyinfo.cpp deleted file mode 100644 index 12f85b969..000000000 --- a/native/MagnetoLib/assemblyinfo.cpp +++ /dev/null @@ -1,38 +0,0 @@ -#include "stdafx.h" - -using namespace System; -using namespace System::Reflection; -using namespace System::Runtime::CompilerServices; -using namespace System::Runtime::InteropServices; -using namespace System::Security::Permissions; - -// -// General Information about an assembly is controlled through the following -// set of attributes. Change these attribute values to modify the information -// associated with an assembly. -// -[assembly:AssemblyTitleAttribute(L"MagnetoLib")]; -[assembly:AssemblyDescriptionAttribute(L"")]; -[assembly:AssemblyConfigurationAttribute(L"")]; -[assembly:AssemblyCompanyAttribute(L"")]; -[assembly:AssemblyProductAttribute(L"MagnetoLib")]; -[assembly:AssemblyCopyrightAttribute(L"Copyright (c) 2021")]; -[assembly:AssemblyTrademarkAttribute(L"")]; -[assembly:AssemblyCultureAttribute(L"")]; - -// -// Version information for an assembly consists of the following four values: -// -// Major Version -// Minor Version -// Build Number -// Revision -// -// You can specify all the value or you can default the Revision and Build Numbers -// by using the '*' as shown below: - -[assembly:AssemblyVersionAttribute("1.0.*")]; - -[assembly:ComVisible(false)]; - -[assembly:CLSCompliantAttribute(true)]; \ No newline at end of file diff --git a/native/MagnetoLib/magneto.h b/native/MagnetoLib/magneto.h deleted file mode 100644 index 4ce7984a6..000000000 --- a/native/MagnetoLib/magneto.h +++ /dev/null @@ -1,32 +0,0 @@ -#pragma once - -// Forward declarations of mymathlib.com routines -void Multiply_Self_Transpose(double*, double*, int, int); -void Get_Submatrix(double*, int, int, double*, int, int, int); -int Choleski_LU_Decomposition(double*, int); -int Choleski_LU_Inverse(double*, int); -void Multiply_Matrices(double*, double*, int, int, double*, int); -void Identity_Matrix(double*, int); - -int Hessenberg_Form_Elementary(double*, double*, int); -static void Hessenberg_Elementary_Transform(double*, double*, int[], int); - -void Copy_Vector(double*, double*, int); - -int QR_Hessenberg_Matrix(double*, double*, double[], double[], int, int); -static void One_Real_Eigenvalue(double[], double[], double[], int, double); -static void Two_Eigenvalues(double*, double*, double[], double[], int, int, double); -static void Update_Row(double*, double, double, int, int); -static void Update_Column(double*, double, double, int, int); -static void Update_Transformation(double*, double, double, int, int); -static void Double_QR_Iteration(double*, double*, int, int, int, double*, int); -static void Product_and_Sum_of_Shifts(double*, int, int, double*, double*, double*, int); -static int Two_Consecutive_Small_Subdiagonal(double*, int, int, int, double, double); -static void Double_QR_Step(double*, int, int, int, double, double, double*, int); -static void BackSubstitution(double*, double[], double[], int); -static void BackSubstitute_Real_Vector(double*, double[], double[], int, double, int); -static void BackSubstitute_Complex_Vector(double*, double[], double[], int, double, int); -static void Calculate_Eigenvectors(double*, double*, double[], double[], int); -static void Complex_Division(double, double, double, double, double*, double*); - -void Transpose_Square_Matrix(double*, int); \ No newline at end of file diff --git a/native/MagnetoLib/magnetolib.h b/native/MagnetoLib/magnetolib.h deleted file mode 100644 index d6db4e07c..000000000 --- a/native/MagnetoLib/magnetolib.h +++ /dev/null @@ -1,33 +0,0 @@ -// MagnetoLib.h -#pragma once - -// Define EXPORTED for any platform -#if defined _WIN32 || defined __CYGWIN__ -#ifdef WIN_EXPORT -// Exporting... -#ifdef __GNUC__ -#define EXPORTED __attribute__ ((dllexport)) -#else -#define EXPORTED __declspec(dllexport) // Note: actually gcc seems to also supports this syntax. -#endif -#else -#ifdef __GNUC__ -#define EXPORTED __attribute__ ((dllimport)) -#else -#define EXPORTED __declspec(dllimport) // Note: actually gcc seems to also supports this syntax. -#endif -#endif -#define NOT_EXPORTED -#else -#if __GNUC__ >= 4 -#define EXPORTED __attribute__ ((visibility ("default"))) -#define NOT_EXPORTED __attribute__ ((visibility ("hidden"))) -#else -#define EXPORTED -#define NOT_EXPORTED -#endif -#endif - -extern "C" EXPORTED void calculate(double data[], int nlines, double nxsrej, double hm, double B[3], double A_1[3 * 3]); - -extern "C" EXPORTED double calculateHnorm(double data[], int nlines); \ No newline at end of file diff --git a/native/MagnetoLib/resource.h b/native/MagnetoLib/resource.h deleted file mode 100644 index d5ac7c42a..000000000 --- a/native/MagnetoLib/resource.h +++ /dev/null @@ -1,3 +0,0 @@ -//{{NO_DEPENDENCIES}} -// Microsoft Visual C++ generated include file. -// Used by app.rc diff --git a/native/MagnetoLib/stdafx.cpp b/native/MagnetoLib/stdafx.cpp deleted file mode 100644 index ffc140e2c..000000000 --- a/native/MagnetoLib/stdafx.cpp +++ /dev/null @@ -1,5 +0,0 @@ -// stdafx.cpp : source file that includes just the standard includes -// MagnetoLib.pch will be the pre-compiled header -// stdafx.obj will contain the pre-compiled type information - -#include "stdafx.h" diff --git a/native/MagnetoLib/stdafx.h b/native/MagnetoLib/stdafx.h deleted file mode 100644 index 66ffb2838..000000000 --- a/native/MagnetoLib/stdafx.h +++ /dev/null @@ -1,13 +0,0 @@ -// stdafx.h : include file for standard system include files, -// or project specific include files that are used frequently, -// but are changed infrequently - -#pragma once - -#include -#include -#include -#include -#include -#include -