nmelone/GoldenCheetah
1
0
Fork 0
mirror of https://github.com/GoldenCheetah/GoldenCheetah.git synced 2026-02-13 16:18:42 +00:00
Code Issues Packages Projects Releases Wiki Activity
Files
d1e2f38e07b3120545ffff5da0138b28fa367050
GoldenCheetah/levmar/Axb_core.c
Mark Liversedge d9b12d63f7 Alternative Levenberg-Marquardt algorithm 
.. allows constrained fits

.. this is a GPL lib that is included into the
   source tree to avoid adding another painful
   deendency.

.. for details of the lib please see:
   http://users.ics.forth.gr/~lourakis/levmar/
2018-08-08 14:34:35 +01:00

1283 lines
36 KiB
C
Raw Blame History

/////////////////////////////////////////////////////////////////////////////////
//
// Solution of linear systems involved in the Levenberg - Marquardt
// minimization algorithm
// Copyright (C) 2004 Manolis Lourakis (lourakis at ics forth gr)
// Institute of Computer Science, Foundation for Research & Technology - Hellas
// Heraklion, Crete, Greece.
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
/////////////////////////////////////////////////////////////////////////////////
/* Solvers for the linear systems Ax=b. Solvers should NOT modify their A & B arguments! */
#ifndef LM_REAL // not included by Axb.c
#error This file should not be compiled directly!
#endif
#ifdef LINSOLVERS_RETAIN_MEMORY
#define __STATIC__ static
#else
#define __STATIC__ // empty
#endif /* LINSOLVERS_RETAIN_MEMORY */
#ifdef HAVE_LAPACK
/* prototypes of LAPACK routines */
#define GEQRF LM_MK_LAPACK_NAME(geqrf)
#define ORGQR LM_MK_LAPACK_NAME(orgqr)
#define TRTRS LM_MK_LAPACK_NAME(trtrs)
#define POTF2 LM_MK_LAPACK_NAME(potf2)
#define POTRF LM_MK_LAPACK_NAME(potrf)
#define POTRS LM_MK_LAPACK_NAME(potrs)
#define GETRF LM_MK_LAPACK_NAME(getrf)
#define GETRS LM_MK_LAPACK_NAME(getrs)
#define GESVD LM_MK_LAPACK_NAME(gesvd)
#define GESDD LM_MK_LAPACK_NAME(gesdd)
#define SYTRF LM_MK_LAPACK_NAME(sytrf)
#define SYTRS LM_MK_LAPACK_NAME(sytrs)
#define PLASMA_POSV LM_CAT_(PLASMA_, LM_ADD_PREFIX(posv))
#ifdef __cplusplus
extern "C" {
#endif
/* QR decomposition */
extern int GEQRF(int *m, int *n, LM_REAL *a, int *lda, LM_REAL *tau, LM_REAL *work, int *lwork, int *info);
extern int ORGQR(int *m, int *n, int *k, LM_REAL *a, int *lda, LM_REAL *tau, LM_REAL *work, int *lwork, int *info);
/* solution of triangular systems */
extern int TRTRS(char *uplo, char *trans, char *diag, int *n, int *nrhs, LM_REAL *a, int *lda, LM_REAL *b, int *ldb, int *info);
/* Cholesky decomposition and systems solution */
extern int POTF2(char *uplo, int *n, LM_REAL *a, int *lda, int *info);
extern int POTRF(char *uplo, int *n, LM_REAL *a, int *lda, int *info); /* block version of dpotf2 */
extern int POTRS(char *uplo, int *n, int *nrhs, LM_REAL *a, int *lda, LM_REAL *b, int *ldb, int *info);
/* LU decomposition and systems solution */
extern int GETRF(int *m, int *n, LM_REAL *a, int *lda, int *ipiv, int *info);
extern int GETRS(char *trans, int *n, int *nrhs, LM_REAL *a, int *lda, int *ipiv, LM_REAL *b, int *ldb, int *info);
/* Singular Value Decomposition (SVD) */
extern int GESVD(char *jobu, char *jobvt, int *m, int *n, LM_REAL *a, int *lda, LM_REAL *s, LM_REAL *u, int *ldu,
LM_REAL *vt, int *ldvt, LM_REAL *work, int *lwork, int *info);
/* lapack 3.0 new SVD routine, faster than xgesvd().
* In case that your version of LAPACK does not include them, use the above two older routines
*/
extern int GESDD(char *jobz, int *m, int *n, LM_REAL *a, int *lda, LM_REAL *s, LM_REAL *u, int *ldu, LM_REAL *vt, int *ldvt,
LM_REAL *work, int *lwork, int *iwork, int *info);
/* LDLt/UDUt factorization and systems solution */
extern int SYTRF(char *uplo, int *n, LM_REAL *a, int *lda, int *ipiv, LM_REAL *work, int *lwork, int *info);
extern int SYTRS(char *uplo, int *n, int *nrhs, LM_REAL *a, int *lda, int *ipiv, LM_REAL *b, int *ldb, int *info);
#ifdef __cplusplus
}
#endif
/* precision-specific definitions */
#define AX_EQ_B_QR LM_ADD_PREFIX(Ax_eq_b_QR)
#define AX_EQ_B_QRLS LM_ADD_PREFIX(Ax_eq_b_QRLS)
#define AX_EQ_B_CHOL LM_ADD_PREFIX(Ax_eq_b_Chol)
#define AX_EQ_B_LU LM_ADD_PREFIX(Ax_eq_b_LU)
#define AX_EQ_B_SVD LM_ADD_PREFIX(Ax_eq_b_SVD)
#define AX_EQ_B_BK LM_ADD_PREFIX(Ax_eq_b_BK)
#define AX_EQ_B_PLASMA_CHOL LM_ADD_PREFIX(Ax_eq_b_PLASMA_Chol)
/*
* This function returns the solution of Ax = b
*
* The function is based on QR decomposition with explicit computation of Q:
* If A=Q R with Q orthogonal and R upper triangular, the linear system becomes
* Q R x = b or R x = Q^T b.
* The last equation can be solved directly.
*
* A is mxm, b is mx1
*
* The function returns 0 in case of error, 1 if successful
*
* This function is often called repetitively to solve problems of identical
* dimensions. To avoid repetitive malloc's and free's, allocated memory is
* retained between calls and free'd-malloc'ed when not of the appropriate size.
* A call with NULL as the first argument forces this memory to be released.
*/
int AX_EQ_B_QR(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)
{
__STATIC__ LM_REAL *buf=NULL;
__STATIC__ int buf_sz=0;
static int nb=0; /* no __STATIC__ decl. here! */
LM_REAL *a, *tau, *r, *work;
int a_sz, tau_sz, r_sz, tot_sz;
register int i, j;
int info, worksz, nrhs=1;
register LM_REAL sum;
if(!A)
#ifdef LINSOLVERS_RETAIN_MEMORY
{
if(buf) free(buf);
buf=NULL;
buf_sz=0;
return 1;
}
#else
return 1; /* NOP */
#endif /* LINSOLVERS_RETAIN_MEMORY */
/* calculate required memory size */
a_sz=m*m;
tau_sz=m;
r_sz=m*m; /* only the upper triangular part really needed */
if(!nb){
LM_REAL tmp;
worksz=-1; // workspace query; optimal size is returned in tmp
GEQRF((int *)&m, (int *)&m, NULL, (int *)&m, NULL, (LM_REAL *)&tmp, (int *)&worksz, (int *)&info);
nb=((int)tmp)/m; // optimal worksize is m*nb
}
worksz=nb*m;
tot_sz=a_sz + tau_sz + r_sz + worksz;
#ifdef LINSOLVERS_RETAIN_MEMORY
if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
if(buf) free(buf); /* free previously allocated memory */
buf_sz=tot_sz;
buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));
if(!buf){
fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_QR) "() failed!\n");
exit(1);
}
}
#else
buf_sz=tot_sz;
buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));
if(!buf){
fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_QR) "() failed!\n");
exit(1);
}
#endif /* LINSOLVERS_RETAIN_MEMORY */
a=buf;
tau=a+a_sz;
r=tau+tau_sz;
work=r+r_sz;
/* store A (column major!) into a */
for(i=0; i<m; i++)
for(j=0; j<m; j++)
a[i+j*m]=A[i*m+j];
/* QR decomposition of A */
GEQRF((int *)&m, (int *)&m, a, (int *)&m, tau, work, (int *)&worksz, (int *)&info);
/* error treatment */
if(info!=0){
if(info<0){
fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", GEQRF) " in ", AX_EQ_B_QR) "()\n", -info);
exit(1);
}
else{
fprintf(stderr, RCAT(RCAT("Unknown LAPACK error %d for ", GEQRF) " in ", AX_EQ_B_QR) "()\n", info);
#ifndef LINSOLVERS_RETAIN_MEMORY
free(buf);
#endif
return 0;
}
}
/* R is stored in the upper triangular part of a; copy it in r so that ORGQR() below won't destroy it */
memcpy(r, a, r_sz*sizeof(LM_REAL));
/* compute Q using the elementary reflectors computed by the above decomposition */
ORGQR((int *)&m, (int *)&m, (int *)&m, a, (int *)&m, tau, work, (int *)&worksz, (int *)&info);
if(info!=0){
if(info<0){
fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", ORGQR) " in ", AX_EQ_B_QR) "()\n", -info);
exit(1);
}
else{
fprintf(stderr, RCAT("Unknown LAPACK error (%d) in ", AX_EQ_B_QR) "()\n", info);
#ifndef LINSOLVERS_RETAIN_MEMORY
free(buf);
#endif
return 0;
}
}
/* Q is now in a; compute Q^T b in x */
for(i=0; i<m; i++){
for(j=0, sum=0.0; j<m; j++)
sum+=a[i*m+j]*B[j];
x[i]=sum;
}
/* solve the linear system R x = Q^t b */
TRTRS("U", "N", "N", (int *)&m, (int *)&nrhs, r, (int *)&m, x, (int *)&m, &info);
/* error treatment */
if(info!=0){
if(info<0){
fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRS) " in ", AX_EQ_B_QR) "()\n", -info);
exit(1);
}
else{
fprintf(stderr, RCAT("LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in ", AX_EQ_B_QR) "()\n", info);
#ifndef LINSOLVERS_RETAIN_MEMORY
free(buf);
#endif
return 0;
}
}
#ifndef LINSOLVERS_RETAIN_MEMORY
free(buf);
#endif
return 1;
}
/*
* This function returns the solution of min_x ||Ax - b||
*
* || . || is the second order (i.e. L2) norm. This is a least squares technique that
* is based on QR decomposition:
* If A=Q R with Q orthogonal and R upper triangular, the normal equations become
* (A^T A) x = A^T b or (R^T Q^T Q R) x = A^T b or (R^T R) x = A^T b.
* This amounts to solving R^T y = A^T b for y and then R x = y for x
* Note that Q does not need to be explicitly computed
*
* A is mxn, b is mx1
*
* The function returns 0 in case of error, 1 if successful
*
* This function is often called repetitively to solve problems of identical
* dimensions. To avoid repetitive malloc's and free's, allocated memory is
* retained between calls and free'd-malloc'ed when not of the appropriate size.
* A call with NULL as the first argument forces this memory to be released.
*/
int AX_EQ_B_QRLS(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m, int n)
{
__STATIC__ LM_REAL *buf=NULL;
__STATIC__ int buf_sz=0;
static int nb=0; /* no __STATIC__ decl. here! */
LM_REAL *a, *tau, *r, *work;
int a_sz, tau_sz, r_sz, tot_sz;
register int i, j;
int info, worksz, nrhs=1;
register LM_REAL sum;
if(!A)
#ifdef LINSOLVERS_RETAIN_MEMORY
{
if(buf) free(buf);
buf=NULL;
buf_sz=0;
return 1;
}
#else
return 1; /* NOP */
#endif /* LINSOLVERS_RETAIN_MEMORY */
if(m<n){
fprintf(stderr, RCAT("Normal equations require that the number of rows is greater than number of columns in ", AX_EQ_B_QRLS) "() [%d x %d]! -- try transposing\n", m, n);
exit(1);
}
/* calculate required memory size */
a_sz=m*n;
tau_sz=n;
r_sz=n*n;
if(!nb){
LM_REAL tmp;
worksz=-1; // workspace query; optimal size is returned in tmp
GEQRF((int *)&m, (int *)&m, NULL, (int *)&m, NULL, (LM_REAL *)&tmp, (int *)&worksz, (int *)&info);
nb=((int)tmp)/m; // optimal worksize is m*nb
}
worksz=nb*m;
tot_sz=a_sz + tau_sz + r_sz + worksz;
#ifdef LINSOLVERS_RETAIN_MEMORY
if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
if(buf) free(buf); /* free previously allocated memory */
buf_sz=tot_sz;
buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));
if(!buf){
fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_QRLS) "() failed!\n");
exit(1);
}
}
#else
buf_sz=tot_sz;
buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));
if(!buf){
fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_QRLS) "() failed!\n");
exit(1);
}
#endif /* LINSOLVERS_RETAIN_MEMORY */
a=buf;
tau=a+a_sz;
r=tau+tau_sz;
work=r+r_sz;
/* store A (column major!) into a */
for(i=0; i<m; i++)
for(j=0; j<n; j++)
a[i+j*m]=A[i*n+j];
/* compute A^T b in x */
for(i=0; i<n; i++){
for(j=0, sum=0.0; j<m; j++)
sum+=A[j*n+i]*B[j];
x[i]=sum;
}
/* QR decomposition of A */
GEQRF((int *)&m, (int *)&n, a, (int *)&m, tau, work, (int *)&worksz, (int *)&info);
/* error treatment */
if(info!=0){
if(info<0){
fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", GEQRF) " in ", AX_EQ_B_QRLS) "()\n", -info);
exit(1);
}
else{
fprintf(stderr, RCAT(RCAT("Unknown LAPACK error %d for ", GEQRF) " in ", AX_EQ_B_QRLS) "()\n", info);
#ifndef LINSOLVERS_RETAIN_MEMORY
free(buf);
#endif
return 0;
}
}
/* R is stored in the upper triangular part of a. Note that a is mxn while r nxn */
for(j=0; j<n; j++){
for(i=0; i<=j; i++)
r[i+j*n]=a[i+j*m];
/* lower part is zero */
for(i=j+1; i<n; i++)
r[i+j*n]=0.0;
}
/* solve the linear system R^T y = A^t b */
TRTRS("U", "T", "N", (int *)&n, (int *)&nrhs, r, (int *)&n, x, (int *)&n, &info);
/* error treatment */
if(info!=0){
if(info<0){
fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRS) " in ", AX_EQ_B_QRLS) "()\n", -info);
exit(1);
}
else{
fprintf(stderr, RCAT("LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in ", AX_EQ_B_QRLS) "()\n", info);
#ifndef LINSOLVERS_RETAIN_MEMORY
free(buf);
#endif
return 0;
}
}
/* solve the linear system R x = y */
TRTRS("U", "N", "N", (int *)&n, (int *)&nrhs, r, (int *)&n, x, (int *)&n, &info);
/* error treatment */
if(info!=0){
if(info<0){
fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRS) " in ", AX_EQ_B_QRLS) "()\n", -info);
exit(1);
}
else{
fprintf(stderr, RCAT("LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in ", AX_EQ_B_QRLS) "()\n", info);
#ifndef LINSOLVERS_RETAIN_MEMORY
free(buf);
#endif
return 0;
}
}
#ifndef LINSOLVERS_RETAIN_MEMORY
free(buf);
#endif
return 1;
}
/*
* This function returns the solution of Ax=b
*
* The function assumes that A is symmetric & postive definite and employs
* the Cholesky decomposition:
* If A=L L^T with L lower triangular, the system to be solved becomes
* (L L^T) x = b
* This amounts to solving L y = b for y and then L^T x = y for x
*
* A is mxm, b is mx1
*
* The function returns 0 in case of error, 1 if successful
*
* This function is often called repetitively to solve problems of identical
* dimensions. To avoid repetitive malloc's and free's, allocated memory is
* retained between calls and free'd-malloc'ed when not of the appropriate size.
* A call with NULL as the first argument forces this memory to be released.
*/
int AX_EQ_B_CHOL(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)
{
__STATIC__ LM_REAL *buf=NULL;
__STATIC__ int buf_sz=0;
LM_REAL *a;
int a_sz, tot_sz;
int info, nrhs=1;
if(!A)
#ifdef LINSOLVERS_RETAIN_MEMORY
{
if(buf) free(buf);
buf=NULL;
buf_sz=0;
return 1;
}
#else
return 1; /* NOP */
#endif /* LINSOLVERS_RETAIN_MEMORY */
/* calculate required memory size */
a_sz=m*m;
tot_sz=a_sz;
#ifdef LINSOLVERS_RETAIN_MEMORY
if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
if(buf) free(buf); /* free previously allocated memory */
buf_sz=tot_sz;
buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));
if(!buf){
fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_CHOL) "() failed!\n");
exit(1);
}
}
#else
buf_sz=tot_sz;
buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));
if(!buf){
fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_CHOL) "() failed!\n");
exit(1);
}
#endif /* LINSOLVERS_RETAIN_MEMORY */
a=buf;
/* store A into a and B into x. A is assumed symmetric,
* hence no transposition is needed
*/
memcpy(a, A, a_sz*sizeof(LM_REAL));
memcpy(x, B, m*sizeof(LM_REAL));
/* Cholesky decomposition of A */
//POTF2("L", (int *)&m, a, (int *)&m, (int *)&info);
POTRF("L", (int *)&m, a, (int *)&m, (int *)&info);
/* error treatment */
if(info!=0){
if(info<0){
fprintf(stderr, RCAT(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", POTF2) "/", POTRF) " in ",
AX_EQ_B_CHOL) "()\n", -info);
exit(1);
}
else{
fprintf(stderr, RCAT(RCAT(RCAT("LAPACK error: the leading minor of order %d is not positive definite,\nthe factorization could not be completed for ", POTF2) "/", POTRF) " in ", AX_EQ_B_CHOL) "()\n", info);
#ifndef LINSOLVERS_RETAIN_MEMORY
free(buf);
#endif
return 0;
}
}
/* solve using the computed Cholesky in one lapack call */
POTRS("L", (int *)&m, (int *)&nrhs, a, (int *)&m, x, (int *)&m, &info);
if(info<0){
fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", POTRS) " in ", AX_EQ_B_CHOL) "()\n", -info);
exit(1);
}
#if 0
/* alternative: solve the linear system L y = b ... */
TRTRS("L", "N", "N", (int *)&m, (int *)&nrhs, a, (int *)&m, x, (int *)&m, &info);
/* error treatment */
if(info!=0){
if(info<0){
fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRS) " in ", AX_EQ_B_CHOL) "()\n", -info);
exit(1);
}
else{
fprintf(stderr, RCAT("LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in ", AX_EQ_B_CHOL) "()\n", info);
#ifndef LINSOLVERS_RETAIN_MEMORY
free(buf);
#endif
return 0;
}
}
/* ... solve the linear system L^T x = y */
TRTRS("L", "T", "N", (int *)&m, (int *)&nrhs, a, (int *)&m, x, (int *)&m, &info);
/* error treatment */
if(info!=0){
if(info<0){
fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRS) "in ", AX_EQ_B_CHOL) "()\n", -info);
exit(1);
}
else{
fprintf(stderr, RCAT("LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in ", AX_EQ_B_CHOL) "()\n", info);
#ifndef LINSOLVERS_RETAIN_MEMORY
free(buf);
#endif
return 0;
}
}
#endif /* 0 */
#ifndef LINSOLVERS_RETAIN_MEMORY
free(buf);
#endif
return 1;
}
#ifdef HAVE_PLASMA
/* Linear algebra using PLASMA parallel library for multicore CPUs.
* http://icl.cs.utk.edu/plasma/
*
* WARNING: BLAS multithreading should be disabled, e.g. setenv MKL_NUM_THREADS 1
*/
#ifndef _LM_PLASMA_MISC_
/* avoid multiple inclusion of helper code */
#define _LM_PLASMA_MISC_
#include <plasma.h>
#include <cblas.h>
#include <lapacke.h>
#include <plasma_tmg.h>
#include <core_blas.h>
/* programmatically determine the number of cores on the current machine */
#ifdef _WIN32
#include <windows.h>
#elif __linux
#include <unistd.h>
#endif
static int getnbcores()
{
#ifdef _WIN32
SYSTEM_INFO sysinfo;
GetSystemInfo(&sysinfo);
return sysinfo.dwNumberOfProcessors;
#elif __linux
return sysconf(_SC_NPROCESSORS_ONLN);
#else // unknown system
return 2<<1; // will be halved by right shift below
#endif
}
static int PLASMA_ncores=-(getnbcores()>>1); // >0 if PLASMA initialized, <0 otherwise
/* user-specified number of cores */
void levmar_PLASMA_setnbcores(int cores)
{
PLASMA_ncores=(cores>0)? -cores : ((cores)? cores : -2);
}
#endif /* _LM_PLASMA_MISC_ */
/*
* This function returns the solution of Ax=b
*
* The function assumes that A is symmetric & positive definite and employs the
* Cholesky decomposition implemented by PLASMA for homogeneous multicore processors.
*
* A is mxm, b is mx1
*
* The function returns 0 in case of error, 1 if successfull
*
* This function is often called repetitively to solve problems of identical
* dimensions. To avoid repetitive malloc's and free's, allocated memory is
* retained between calls and free'd-malloc'ed when not of the appropriate size.
* A call with NULL as the first argument forces this memory to be released.
*/
int AX_EQ_B_PLASMA_CHOL(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)
{
__STATIC__ LM_REAL *buf=NULL;
__STATIC__ int buf_sz=0;
LM_REAL *a;
int a_sz, tot_sz;
int info, nrhs=1;
if(A==NULL){
#ifdef LINSOLVERS_RETAIN_MEMORY
if(buf) free(buf);
buf=NULL;
buf_sz=0;
#endif /* LINSOLVERS_RETAIN_MEMORY */
PLASMA_Finalize();
PLASMA_ncores=-PLASMA_ncores;
return 1;
}
/* calculate required memory size */
a_sz=m*m;
tot_sz=a_sz;
#ifdef LINSOLVERS_RETAIN_MEMORY
if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
if(buf) free(buf); /* free previously allocated memory */
buf_sz=tot_sz;
buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));
if(!buf){
fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_PLASMA_CHOL) "() failed!\n");
exit(1);
}
}
#else
buf_sz=tot_sz;
buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));
if(!buf){
fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_PLASMA_CHOL) "() failed!\n");
exit(1);
}
#endif /* LINSOLVERS_RETAIN_MEMORY */
a=buf;
/* store A into a and B into x; A is assumed to be symmetric,
* hence no transposition is needed
*/
memcpy(a, A, a_sz*sizeof(LM_REAL));
memcpy(x, B, m*sizeof(LM_REAL));
/* initialize PLASMA */
if(PLASMA_ncores<0){
PLASMA_ncores=-PLASMA_ncores;
PLASMA_Init(PLASMA_ncores);
fprintf(stderr, RCAT("\n", AX_EQ_B_PLASMA_CHOL) "(): PLASMA is running on %d cores.\n\n", PLASMA_ncores);
}
/* Solve the linear system */
info=PLASMA_POSV(PlasmaLower, m, 1, a, m, x, m);
/* error treatment */
if(info!=0){
if(info<0){
fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", PLASMA_POSV) " in ",
AX_EQ_B_PLASMA_CHOL) "()\n", -info);
exit(1);
}
else{
fprintf(stderr, RCAT(RCAT("LAPACK error: the leading minor of order %d is not positive definite,\n"
"the factorization could not be completed for ", PLASMA_POSV) " in ", AX_EQ_B_CHOL) "()\n", info);
#ifndef LINSOLVERS_RETAIN_MEMORY
free(buf);
#endif
return 0;
}
}
#ifndef LINSOLVERS_RETAIN_MEMORY
free(buf);
#endif
return 1;
}
#endif /* HAVE_PLASMA */
/*
* This function returns the solution of Ax = b
*
* The function employs LU decomposition:
* If A=L U with L lower and U upper triangular, then the original system
* amounts to solving
* L y = b, U x = y
*
* A is mxm, b is mx1
*
* The function returns 0 in case of error, 1 if successful
*
* This function is often called repetitively to solve problems of identical
* dimensions. To avoid repetitive malloc's and free's, allocated memory is
* retained between calls and free'd-malloc'ed when not of the appropriate size.
* A call with NULL as the first argument forces this memory to be released.
*/
int AX_EQ_B_LU(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)
{
__STATIC__ LM_REAL *buf=NULL;
__STATIC__ int buf_sz=0;
int a_sz, ipiv_sz, tot_sz;
register int i, j;
int info, *ipiv, nrhs=1;
LM_REAL *a;
if(!A)
#ifdef LINSOLVERS_RETAIN_MEMORY
{
if(buf) free(buf);
buf=NULL;
buf_sz=0;
return 1;
}
#else
return 1; /* NOP */
#endif /* LINSOLVERS_RETAIN_MEMORY */
/* calculate required memory size */
ipiv_sz=m;
a_sz=m*m;
tot_sz=a_sz*sizeof(LM_REAL) + ipiv_sz*sizeof(int); /* should be arranged in that order for proper doubles alignment */
#ifdef LINSOLVERS_RETAIN_MEMORY
if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
if(buf) free(buf); /* free previously allocated memory */
buf_sz=tot_sz;
buf=(LM_REAL *)malloc(buf_sz);
if(!buf){
fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_LU) "() failed!\n");
exit(1);
}
}
#else
buf_sz=tot_sz;
buf=(LM_REAL *)malloc(buf_sz);
if(!buf){
fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_LU) "() failed!\n");
exit(1);
}
#endif /* LINSOLVERS_RETAIN_MEMORY */
a=buf;
ipiv=(int *)(a+a_sz);
/* store A (column major!) into a and B into x */
for(i=0; i<m; i++){
for(j=0; j<m; j++)
a[i+j*m]=A[i*m+j];
x[i]=B[i];
}
/* LU decomposition for A */
GETRF((int *)&m, (int *)&m, a, (int *)&m, ipiv, (int *)&info);
if(info!=0){
if(info<0){
fprintf(stderr, RCAT(RCAT("argument %d of ", GETRF) " illegal in ", AX_EQ_B_LU) "()\n", -info);
exit(1);
}
else{
fprintf(stderr, RCAT(RCAT("singular matrix A for ", GETRF) " in ", AX_EQ_B_LU) "()\n");
#ifndef LINSOLVERS_RETAIN_MEMORY
free(buf);
#endif
return 0;
}
}
/* solve the system with the computed LU */
GETRS("N", (int *)&m, (int *)&nrhs, a, (int *)&m, ipiv, x, (int *)&m, (int *)&info);
if(info!=0){
if(info<0){
fprintf(stderr, RCAT(RCAT("argument %d of ", GETRS) " illegal in ", AX_EQ_B_LU) "()\n", -info);
exit(1);
}
else{
fprintf(stderr, RCAT(RCAT("unknown error for ", GETRS) " in ", AX_EQ_B_LU) "()\n");
#ifndef LINSOLVERS_RETAIN_MEMORY
free(buf);
#endif
return 0;
}
}
#ifndef LINSOLVERS_RETAIN_MEMORY
free(buf);
#endif
return 1;
}
/*
* This function returns the solution of Ax = b
*
* The function is based on SVD decomposition:
* If A=U D V^T with U, V orthogonal and D diagonal, the linear system becomes
* (U D V^T) x = b or x=V D^{-1} U^T b
* Note that V D^{-1} U^T is the pseudoinverse A^+
*
* A is mxm, b is mx1.
*
* The function returns 0 in case of error, 1 if successful
*
* This function is often called repetitively to solve problems of identical
* dimensions. To avoid repetitive malloc's and free's, allocated memory is
* retained between calls and free'd-malloc'ed when not of the appropriate size.
* A call with NULL as the first argument forces this memory to be released.
*/
int AX_EQ_B_SVD(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)
{
__STATIC__ LM_REAL *buf=NULL;
__STATIC__ int buf_sz=0;
static LM_REAL eps=LM_CNST(-1.0);
register int i, j;
LM_REAL *a, *u, *s, *vt, *work;
int a_sz, u_sz, s_sz, vt_sz, tot_sz;
LM_REAL thresh, one_over_denom;
register LM_REAL sum;
int info, rank, worksz, *iwork, iworksz;
if(!A)
#ifdef LINSOLVERS_RETAIN_MEMORY
{
if(buf) free(buf);
buf=NULL;
buf_sz=0;
return 1;
}
#else
return 1; /* NOP */
#endif /* LINSOLVERS_RETAIN_MEMORY */
/* calculate required memory size */
#if 1 /* use optimal size */
worksz=-1; // workspace query. Keep in mind that GESDD requires more memory than GESVD
/* note that optimal work size is returned in thresh */
GESVD("A", "A", (int *)&m, (int *)&m, NULL, (int *)&m, NULL, NULL, (int *)&m, NULL, (int *)&m, (LM_REAL *)&thresh, (int *)&worksz, &info);
//GESDD("A", (int *)&m, (int *)&m, NULL, (int *)&m, NULL, NULL, (int *)&m, NULL, (int *)&m, (LM_REAL *)&thresh, (int *)&worksz, NULL, &info);
worksz=(int)thresh;
#else /* use minimum size */
worksz=5*m; // min worksize for GESVD
//worksz=m*(7*m+4); // min worksize for GESDD
#endif
iworksz=8*m;
a_sz=m*m;
u_sz=m*m; s_sz=m; vt_sz=m*m;
tot_sz=(a_sz + u_sz + s_sz + vt_sz + worksz)*sizeof(LM_REAL) + iworksz*sizeof(int); /* should be arranged in that order for proper doubles alignment */
#ifdef LINSOLVERS_RETAIN_MEMORY
if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
if(buf) free(buf); /* free previously allocated memory */
buf_sz=tot_sz;
buf=(LM_REAL *)malloc(buf_sz);
if(!buf){
fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_SVD) "() failed!\n");
exit(1);
}
}
#else
buf_sz=tot_sz;
buf=(LM_REAL *)malloc(buf_sz);
if(!buf){
fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_SVD) "() failed!\n");
exit(1);
}
#endif /* LINSOLVERS_RETAIN_MEMORY */
a=buf;
u=a+a_sz;
s=u+u_sz;
vt=s+s_sz;
work=vt+vt_sz;
iwork=(int *)(work+worksz);
/* store A (column major!) into a */
for(i=0; i<m; i++)
for(j=0; j<m; j++)
a[i+j*m]=A[i*m+j];
/* SVD decomposition of A */
GESVD("A", "A", (int *)&m, (int *)&m, a, (int *)&m, s, u, (int *)&m, vt, (int *)&m, work, (int *)&worksz, &info);
//GESDD("A", (int *)&m, (int *)&m, a, (int *)&m, s, u, (int *)&m, vt, (int *)&m, work, (int *)&worksz, iwork, &info);
/* error treatment */
if(info!=0){
if(info<0){
fprintf(stderr, RCAT(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", GESVD), "/" GESDD) " in ", AX_EQ_B_SVD) "()\n", -info);
exit(1);
}
else{
fprintf(stderr, RCAT("LAPACK error: dgesdd (dbdsdc)/dgesvd (dbdsqr) failed to converge in ", AX_EQ_B_SVD) "() [info=%d]\n", info);
#ifndef LINSOLVERS_RETAIN_MEMORY
free(buf);
#endif
return 0;
}
}
if(eps<0.0){
LM_REAL aux;
/* compute machine epsilon */
for(eps=LM_CNST(1.0); aux=eps+LM_CNST(1.0), aux-LM_CNST(1.0)>0.0; eps*=LM_CNST(0.5))
;
eps*=LM_CNST(2.0);
}
/* compute the pseudoinverse in a */
for(i=0; i<a_sz; i++) a[i]=0.0; /* initialize to zero */
for(rank=0, thresh=eps*s[0]; rank<m && s[rank]>thresh; rank++){
one_over_denom=LM_CNST(1.0)/s[rank];
for(j=0; j<m; j++)
for(i=0; i<m; i++)
a[i*m+j]+=vt[rank+i*m]*u[j+rank*m]*one_over_denom;
}
/* compute A^+ b in x */
for(i=0; i<m; i++){
for(j=0, sum=0.0; j<m; j++)
sum+=a[i*m+j]*B[j];
x[i]=sum;
}
#ifndef LINSOLVERS_RETAIN_MEMORY
free(buf);
#endif
return 1;
}
/*
* This function returns the solution of Ax = b for a real symmetric matrix A
*
* The function is based on LDLT factorization with the pivoting
* strategy of Bunch and Kaufman:
* A is factored as L*D*L^T where L is lower triangular and
* D symmetric and block diagonal (aka spectral decomposition,
* Banachiewicz factorization, modified Cholesky factorization)
*
* A is mxm, b is mx1.
*
* The function returns 0 in case of error, 1 if successfull
*
* This function is often called repetitively to solve problems of identical
* dimensions. To avoid repetitive malloc's and free's, allocated memory is
* retained between calls and free'd-malloc'ed when not of the appropriate size.
* A call with NULL as the first argument forces this memory to be released.
*/
int AX_EQ_B_BK(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)
{
__STATIC__ LM_REAL *buf=NULL;
__STATIC__ int buf_sz=0, nb=0;
LM_REAL *a, *work;
int a_sz, ipiv_sz, work_sz, tot_sz;
int info, *ipiv, nrhs=1;
if(!A)
#ifdef LINSOLVERS_RETAIN_MEMORY
{
if(buf) free(buf);
buf=NULL;
buf_sz=0;
return 1;
}
#else
return 1; /* NOP */
#endif /* LINSOLVERS_RETAIN_MEMORY */
/* calculate required memory size */
ipiv_sz=m;
a_sz=m*m;
if(!nb){
LM_REAL tmp;
work_sz=-1; // workspace query; optimal size is returned in tmp
SYTRF("L", (int *)&m, NULL, (int *)&m, NULL, (LM_REAL *)&tmp, (int *)&work_sz, (int *)&info);
nb=((int)tmp)/m; // optimal worksize is m*nb
}
work_sz=(nb!=-1)? nb*m : 1;
tot_sz=(a_sz + work_sz)*sizeof(LM_REAL) + ipiv_sz*sizeof(int); /* should be arranged in that order for proper doubles alignment */
#ifdef LINSOLVERS_RETAIN_MEMORY
if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
if(buf) free(buf); /* free previously allocated memory */
buf_sz=tot_sz;
buf=(LM_REAL *)malloc(buf_sz);
if(!buf){
fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_BK) "() failed!\n");
exit(1);
}
}
#else
buf_sz=tot_sz;
buf=(LM_REAL *)malloc(buf_sz);
if(!buf){
fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_BK) "() failed!\n");
exit(1);
}
#endif /* LINSOLVERS_RETAIN_MEMORY */
a=buf;
work=a+a_sz;
ipiv=(int *)(work+work_sz);
/* store A into a and B into x; A is assumed to be symmetric, hence
* the column and row major order representations are the same
*/
memcpy(a, A, a_sz*sizeof(LM_REAL));
memcpy(x, B, m*sizeof(LM_REAL));
/* LDLt factorization for A */
SYTRF("L", (int *)&m, a, (int *)&m, ipiv, work, (int *)&work_sz, (int *)&info);
if(info!=0){
if(info<0){
fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", SYTRF) " in ", AX_EQ_B_BK) "()\n", -info);
exit(1);
}
else{
fprintf(stderr, RCAT(RCAT("LAPACK error: singular block diagonal matrix D for", SYTRF) " in ", AX_EQ_B_BK)"() [D(%d, %d) is zero]\n", info, info);
#ifndef LINSOLVERS_RETAIN_MEMORY
free(buf);
#endif
return 0;
}
}
/* solve the system with the computed factorization */
SYTRS("L", (int *)&m, (int *)&nrhs, a, (int *)&m, ipiv, x, (int *)&m, (int *)&info);
if(info<0){
fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", SYTRS) " in ", AX_EQ_B_BK) "()\n", -info);
exit(1);
}
#ifndef LINSOLVERS_RETAIN_MEMORY
free(buf);
#endif
return 1;
}
/* undefine all. IT MUST REMAIN IN THIS POSITION IN FILE */
#undef AX_EQ_B_QR
#undef AX_EQ_B_QRLS
#undef AX_EQ_B_CHOL
#undef AX_EQ_B_LU
#undef AX_EQ_B_SVD
#undef AX_EQ_B_BK
#undef AX_EQ_B_PLASMA_CHOL
#undef GEQRF
#undef ORGQR
#undef TRTRS
#undef POTF2
#undef POTRF
#undef POTRS
#undef GETRF
#undef GETRS
#undef GESVD
#undef GESDD
#undef SYTRF
#undef SYTRS
#undef PLASMA_POSV
#else // no LAPACK
/* precision-specific definitions */
#define AX_EQ_B_LU LM_ADD_PREFIX(Ax_eq_b_LU_noLapack)
/*
* This function returns the solution of Ax = b
*
* The function employs LU decomposition followed by forward/back substitution (see
* also the LAPACK-based LU solver above)
*
* A is mxm, b is mx1
*
* The function returns 0 in case of error, 1 if successful
*
* This function is often called repetitively to solve problems of identical
* dimensions. To avoid repetitive malloc's and free's, allocated memory is
* retained between calls and free'd-malloc'ed when not of the appropriate size.
* A call with NULL as the first argument forces this memory to be released.
*/
int AX_EQ_B_LU(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)
{
__STATIC__ void *buf=NULL;
__STATIC__ int buf_sz=0;
register int i, j, k;
int *idx, maxi=-1, idx_sz, a_sz, work_sz, tot_sz;
LM_REAL *a, *work, max, sum, tmp;
if(!A)
#ifdef LINSOLVERS_RETAIN_MEMORY
{
if(buf) free(buf);
buf=NULL;
buf_sz=0;
return 1;
}
#else
return 1; /* NOP */
#endif /* LINSOLVERS_RETAIN_MEMORY */
/* calculate required memory size */
idx_sz=m;
a_sz=m*m;
work_sz=m;
tot_sz=(a_sz+work_sz)*sizeof(LM_REAL) + idx_sz*sizeof(int); /* should be arranged in that order for proper doubles alignment */
#ifdef LINSOLVERS_RETAIN_MEMORY
if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
if(buf) free(buf); /* free previously allocated memory */
buf_sz=tot_sz;
buf=(void *)malloc(tot_sz);
if(!buf){
fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_LU) "() failed!\n");
exit(1);
}
}
#else
buf_sz=tot_sz;
buf=(void *)malloc(tot_sz);
if(!buf){
fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_LU) "() failed!\n");
exit(1);
}
#endif /* LINSOLVERS_RETAIN_MEMORY */
a=buf;
work=a+a_sz;
idx=(int *)(work+work_sz);
/* avoid destroying A, B by copying them to a, x resp. */
memcpy(a, A, a_sz*sizeof(LM_REAL));
memcpy(x, B, m*sizeof(LM_REAL));
/* compute the LU decomposition of a row permutation of matrix a; the permutation itself is saved in idx[] */
for(i=0; i<m; ++i){
max=0.0;
for(j=0; j<m; ++j)
if((tmp=FABS(a[i*m+j]))>max)
max=tmp;
if(max==0.0){
fprintf(stderr, RCAT("Singular matrix A in ", AX_EQ_B_LU) "()!\n");
#ifndef LINSOLVERS_RETAIN_MEMORY
free(buf);
#endif
return 0;
}
work[i]=LM_CNST(1.0)/max;
}
for(j=0; j<m; ++j){
for(i=0; i<j; ++i){
sum=a[i*m+j];
for(k=0; k<i; ++k)
sum-=a[i*m+k]*a[k*m+j];
a[i*m+j]=sum;
}
max=0.0;
for(i=j; i<m; ++i){
sum=a[i*m+j];
for(k=0; k<j; ++k)
sum-=a[i*m+k]*a[k*m+j];
a[i*m+j]=sum;
if((tmp=work[i]*FABS(sum))>=max){
max=tmp;
maxi=i;
}
}
if(j!=maxi){
for(k=0; k<m; ++k){
tmp=a[maxi*m+k];
a[maxi*m+k]=a[j*m+k];
a[j*m+k]=tmp;
}
work[maxi]=work[j];
}
idx[j]=maxi;
if(a[j*m+j]==0.0)
a[j*m+j]=LM_REAL_EPSILON;
if(j!=m-1){
tmp=LM_CNST(1.0)/(a[j*m+j]);
for(i=j+1; i<m; ++i)
a[i*m+j]*=tmp;
}
}
/* The decomposition has now replaced a. Solve the linear system using
* forward and back substitution
*/
for(i=k=0; i<m; ++i){
j=idx[i];
sum=x[j];
x[j]=x[i];
if(k!=0)
for(j=k-1; j<i; ++j)
sum-=a[i*m+j]*x[j];
else
if(sum!=0.0)
k=i+1;
x[i]=sum;
}
for(i=m-1; i>=0; --i){
sum=x[i];
for(j=i+1; j<m; ++j)
sum-=a[i*m+j]*x[j];
x[i]=sum/a[i*m+i];
}
#ifndef LINSOLVERS_RETAIN_MEMORY
free(buf);
#endif
return 1;
}
/* undefine all. IT MUST REMAIN IN THIS POSITION IN FILE */
#undef AX_EQ_B_LU
#endif /* HAVE_LAPACK */
Reference in New Issue View Git Blame Copy Permalink