📄 rlp_math.cpp
字号:
//rlp_math.cpp, Copyright (c) 2004, 2005 R.Lackner
//
// This file is part of RLPlot.
//
// RLPlot 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.
//
// RLPlot 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.
//
// You should have received a copy of the GNU General Public License
// along with RLPlot; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
//
#include "rlplot.h"
#include <math.h>
#include <stdlib.h>
#define SWAP(a,b) {double temp=(a);(a)=(b);(b)=temp;}
#define _PREC 1.0e-12
static char *MRQ_error = 0L;
//---------------------------------------------------------------------------
//utilitity functions for memory allocation
double **dmatrix(int nrl, int nrh, int ncl, int nch)
{
int i;
double **m;
m = (double **)malloc(nrh * sizeof(double*));
//Allocate rows and set pointers to them
for(i = 0; i < nrh; i++) {
m[i] = (double *)malloc(nrh * sizeof(double));
}
return m;
}
void free_dmatrix(double **m, int nrl, int nrh, int ncl, int)
{
int i;
for(i = 0; i < nrh; i++) free(m[i]);
free(m);
}
//---------------------------------------------------------------------------
//The routine gaussj solves linear equations by Gauss-Jordan elimination
bool gaussj(double **a, int n, double **b, int m)
{
int *indxc, *indxr, *ipiv;
int i, icol, irow, j, k, l, ll;
double big, dum, pivinv;
indxc = (int*)malloc(n*sizeof(int*));
indxr = (int*)malloc(n*sizeof(int*));
ipiv = (int*)malloc(n*sizeof(int*));
for (j = 0; j < n; j++) ipiv[j] = 0;
for (i = 0; i < n; i++) { //This is the main loop over the
big = 0.0; // columns to be reduced
for(j = 0; j < n; j ++) //This is the outer loop of the search
if(ipiv[j] != 1) // for a pivot element
for(k = 0; k < n; k ++) {
if (ipiv[k] == 0) {
if(fabs(a[j][k]) >= big) {
big = fabs(a[j][k]);
irow = j; icol = k;
}
}
else if(ipiv[k] > 1) {
MRQ_error = "Singular Matrix (1)";
free(ipiv); free(indxr); free(indxc);
return false;
}
}
++(ipiv[icol]);
//We now have the pivot element, so we interchange rows, if needed,
// to put the pivot element on the diagonal.
if(irow != icol) {
for(l = 0; l < n; l++) SWAP(a[irow][l], a[icol][l])
for(l = 0; l < m; l++) SWAP(b[irow][l], b[icol][l])
}
indxr[i] = irow; indxc[i] = icol;
if(a[icol][icol] == 0.0) {
MRQ_error = "Singular Matrix (2)";
free(ipiv); free(indxr); free(indxc);
return false;
}
pivinv = 1.0/a[icol][icol];
a[icol][icol] = 1.0;
for(l = 0; l < n; l++) a[icol][l] *= pivinv;
for(l = 0; l < m; l++) b[icol][l] *= pivinv;
for(ll = 0; ll < n; ll++)
if(ll != icol) { //Next, we reduce the rows
dum = a[ll][icol];
a[ll][icol] = 0.0;
for(l = 0; l < n; l++) a[ll][l] -= a[icol][l]*dum;
for(l = 0; l < m; l++) b[ll][l] -= b[icol][l]*dum;
}
} // This is the end of the main loop
for (l = n; l > 0; l--) { // over columns of the reduction.
if(indxr[l] != indxc[l]) // Unscramble the solution
for(k = 0; k < n; k++) SWAP (a[k][indxr[l]], a[k][indxc[l]]);
} //And we are done.
free(ipiv); free(indxr); free(indxc);
return true;
}
//---------------------------------------------------------------------------
//The routine mrqcof is called by mrqmin to evaluate the linearized fitting
// matrix alpha and vector beta
void mrqcof(double x[], double y[], double z[], int ndata, double **a, int ma,
int lista[], int mfit, double **alpha, double beta[], double *chisq,
void (*funcs)(double, double, double **, double *, double *, int))
{
int k, j, i;
double ymod, wt, dy;
double *dyda;
dyda = (double*)malloc(ma*sizeof(double));
for(j = 0; j < mfit; j++) { //Initialize (symmetric) alpha, beta
for(k = 0; k <= j; k++) alpha[j][k] = 0.0;
beta[j] = 0.0;
}
*chisq = 0.0;
for (i = 0; i < ndata; i++) { //Summation loop over all data
(*funcs)(x[i], z ? z[i] : 0.0, a, &ymod, dyda, ma);
if(ymod != 0.0) dy = y[i]-ymod; //functions = 0.0 if out of range
else dy = 0.0;
for(j = 0; j < mfit; j++) {
wt = dyda[lista[j]];
for (k = 0; k <= j; k++){
alpha[j][k] += wt*dyda[lista[k]];
}
beta[j] += dy*wt;
}
(*chisq) += dy*dy; //And find X^2 if function o.k.
}
for(j = 0; j < mfit; j++) //Fill the symmetric side
for(k = 0; k <= j; k++) alpha[k][j]=alpha[j][k];
free(dyda);
}
//---------------------------------------------------------------------------
//The routine mrqmin performs one iteration of Marquart's method for nonlinear
// parameter estimation
bool mrqmin(double *x, double *y, double *z, int ndata, double **a, int ma,
int *lista, int mfit, double **covar, double **alpha, double *chisq,
void (*funcs)(double, double, double **, double *, double *, int), double *alamda)
{
int k, kk, j, ihit;
static double *da, *atry, *beta, ochisq;
static double **oneda, **atryref;
if (*alamda < 0.0) { //Initialization
MRQ_error = 0L;
oneda = dmatrix(1, mfit, 1, 1);
atry = (double *)malloc(ma * sizeof(double));
atryref = (double**)malloc(ma * sizeof(double*));
for(j=0; j < ma; atryref[j++] = &atry[j]);
da = (double*)malloc(ma *sizeof(double));
beta = (double*)malloc(ma *sizeof(double));
kk = mfit+1;
for(j = 0; j < ma; j++) { //Does lista contain a proper
ihit = 0; // permutation of the
for(k = 0; k < mfit; k++) // coefficients ?
if(lista[k] == j) ihit++;
if(ihit == 0)
lista[kk++] = j;
else if (ihit >1) ErrorBox("Bad LISTA permutations in MRQMIN-1");
}
if(kk != ma+1) ErrorBox("Bad LISTA permutations in MRQMIN-2");
*alamda = 0.001;
mrqcof(x, y, z, ndata, a, ma, lista, mfit, alpha, beta, chisq, funcs);
ochisq=(*chisq);
}
for (j = 0; j < mfit; j++) { //Alter linearized fitting matrix
for(k = 0; k < mfit; k++) covar[j][k] = alpha[j][k]; // by augmenting
covar[j][j] = alpha[j][j]*(1.0+(*alamda)); // diagaonal elements
oneda[j][0] = beta[j];
}
if (!gaussj(covar, mfit, oneda, 1)) return false; //Matrix solution ?
for(j = 0; j < mfit; j++) da[j] = oneda[j][0];
if(*alamda == 0.0) { //Once converged evaluate
// covariance matrix with
free(beta); // alamda = 0.
free(da);
free(atry);
free(atryref);
free_dmatrix(oneda, 1, mfit, 1, 1);
return true;
}
for(j = 0; j < ma; j++) atry[j] = *a[j];
for(j = 0; j < mfit; j++) //Did the trial succeed ?
atry[lista[j]] = *a[lista[j]] + da[j];
mrqcof(x, y, z, ndata, atryref, ma, lista, mfit, covar, da, chisq, funcs);
if(*chisq < ochisq) { //Success, accept the new solution
*alamda *= 0.1;
ochisq=(*chisq);
for(j = 0; j < mfit; j++) {
for(k = 0; k < mfit; k++) alpha[j][k] = covar[j][k];
beta[j] = da[j];
*a[lista[j]] = atry[lista[j]];
}
}
else { //Failure, increase almda and
*alamda *= 10.0; // return.
*chisq = ochisq;
}
return true;
}
bool Check_MRQerror()
{
bool bRet;
if(bRet = MRQ_error != 0L) ErrorBox(MRQ_error);
MRQ_error = 0L;
return bRet;
}
//---------------------------------------------------------------------------
//Use heap sort to sort elements of an float array
//W.H. pres, B.P. Flannery, S.A. Teukolsky, W.T. Vetterling (1988/1989)
//Numerical Recipes in C, Cambridge University Press, ISBN 0-521-35465-X
// p. 245
void SortArray(int n, double *vals)
{
int l, j, ir, i;
double rra, *ra = vals-1;
if(n < 2 || !vals) return;
l=(n >> 1) + 1; ir = n;
for( ; ; ) {
if(l > 1) rra = ra[--l];
else {
rra = ra[ir]; ra[ir] = ra[1];
if(--ir == 1) {
ra[1] = rra; return;
}
}
i = l; j = l << 1;
while (j <= ir) {
if (j < ir && ra[j] < ra[j+1]) ++j;
if (rra < ra[j]) {
ra[i] = ra[j]; j += (i=j);
}
else j = ir + 1;
}
ra[i] = rra;
}
}
//sorts array v1 making the corresponding rearrangement of v2
void SortArray2(int n, double *v1, double *v2)
{
int l, j, ir, i;
double rra, rrb, *ra = v1-1, *rb = v2-1;
if(n < 2 || !v1 || !v2) return;
l=(n >> 1) + 1; ir = n;
for( ; ; ) {
if(l > 1) {
rra = ra[--l]; rrb = rb[l];
}
else {
rra = ra[ir]; rrb = rb[ir];
ra[ir] = ra[1]; rb[ir] = rb[1];
if(--ir == 1) {
ra[1] = rra; rb[1] = rrb;
return;
}
}
i = l; j = l << 1;
while (j <= ir) {
if (j < ir && ra[j] < ra[j+1]) ++j;
if (rra < ra[j]) {
ra[i] = ra[j]; rb[i] = rb[j];
j += (i=j);
}
else j = ir + 1;
}
ra[i] = rra; rb[i] = rrb;
}
}
//Use heap sort to sort elements of an xy array
void SortFpArray(int n, lfPOINT *vals)
{
int l, j, ir, i;
lfPOINT rra, *ra = vals-1;
if(n < 2) return;
l=(n >> 1) + 1; ir = n;
for( ; ; ) {
if(l > 1) {
rra.fx = ra[--l].fx; rra.fy = ra[l].fy;
}
else {
rra.fx = ra[ir].fx; rra.fy = ra[ir].fy;
ra[ir].fx = ra[1].fx; ra[ir].fy = ra[1].fy;
if(--ir == 1) {
ra[1].fx = rra.fx; ra[1].fy = rra.fy;
return;
}
}
i = l; j = l << 1;
while (j <= ir) {
if (j < ir && ra[j].fx < ra[j+1].fx) ++j;
if (rra.fx < ra[j].fx) {
ra[i].fx = ra[j].fx; ra[i].fy = ra[j].fy;
j += (i=j);
}
else j = ir + 1;
}
ra[i].fx = rra.fx; ra[i].fy = rra.fy;
}
}
//---------------------------------------------------------------------------
// Cubic Spline Interpolation
// Ref.: W.H. Press, B.P. Flannery, S.A. Teukolsky, W.T. Vetterling (1989),
// Numerical Rcipies in C. The Art of Scientific Computing,
// Cambridge University Press, ISBN 0-521-35465, pp. 96 ff.
void spline(lfPOINT *v, int n, double *y2)
{
int i, k;
double p, qn, sig, un, *u;
u = (double *)malloc(n * sizeof(double));
y2[0] = u[0] = 0.0;
for(i = 1; i < (n-1); i++) {
sig = (v[i].fx-v[i-1].fx)/(v[i+1].fx-v[i-1].fx);
p = sig*y2[i-1]+2.0; y2[i]=(sig-1.0)/p;
u[i]=(v[i+1].fy-v[i].fy)/(v[i+1].fx-v[i].fx)-(v[i].fy-v[i-1].fy)/(v[i].fx-v[i-1].fx);
u[i]=(6.0*u[i]/(v[i+1].fx-v[i-1].fx)-sig*u[i-1])/p;
}
qn = un = 0.0;
y2[n-1] = (un - qn * u[n-2])/(qn*y2[n-2]+1.0);
for(k = n-2; k >= 0; k--) {
y2[k] = y2[k]*y2[k+1]+u[k];
}
free(u);
}
//---------------------------------------------------------------------------
// Special Functions
// Ref.: W.H. Press, B.P. Flannery, S.A. Teukolsky, W.T. Vetterling (1989),
// Numerical Rcipies in C. The Art of Scientific Computing,
// Cambridge University Press, ISBN 0-521-35465, pp. 166 ff.
// The Gamma Function: return the ln(G(xx)) for xx > 0
double gammln(double xx)
{
double x, tmp, ser;
static double cof[6] = {76.18009173, -86.50532033, 24.01409822,
-1.231739516, 0.120858003e-2, -0.536382e-5};
int j;
if(xx < 0.0) return 0.0;
x = xx-1; tmp = x + 5.5; tmp -= (x + 0.5)*log(tmp);
for (j = 0, ser = 1.0; j <= 5; j++) {
x += 1.0; ser += cof[j]/x;
}
return -tmp + log(2.50662827465*ser);
}
//The Factorial Function: return n!
double factrl(int n)
{
static int ntop = 4;
static double a[33]={1.0, 1.0, 2.0, 6.0, 24.0};
int j;
if(n < 0) return 0.0; //error: no factorial for negative numbers
if(n > 32) return exp(gammln(n+1.0));
while(ntop < n) { //fill in table up to desired value
j = ntop++; a[ntop]=a[j] * ntop;
}
return a[n];
}
//returns the incomplete gamma function evaluated by its series representation
void gser(double *gamser, double a, double x, double *gln)
{
int n;
double sum, del, ap;
*gln = gammln(a);
if(x <= 0) {
*gamser = 0.0; return;
}
else {
ap = a; del = sum = 1.0/a;
for(n = 1; n <= 100; n++) {
ap += 1.0; del *= x/ap; sum += del;
if(fabs(del) <= fabs(sum) * _PREC) {
*gamser = sum * exp(-x + a * log(x)-(*gln));
return;
}
}
// maximum number of iterations exceeded
*gamser = sum * exp(-x + a * log(x)-(*gln));
}
}
//returns the incomplete gamma function evaluated by its continued fraction representation
void gcf(double *gammcf, double a, double x, double *gln)
{
int n;
double gold=0.0, g, fac=1.0, b1=1.0, b0=0.0, anf, ana, an, a1, a0=1.0;
*gln=gammln(a); a1=x;
for(n=1; n <= 100; n++) {
an = (double)n; ana = an -a; a0 = (a1 + a0 * ana) * fac;
b0 = (b1 + b0 * ana) *fac; anf = an * fac;
a1 = x * a0 + anf * a1; b1 = x * b0 + anf * b1;
if(a1) {
fac = 1.0 / a1; g = b1 * fac;
if(fabs((g-gold)/g) <= _PREC) {
*gammcf = exp(-x + a * log(x) -(*gln)) * g;
return;
}
gold = g;
}
}
// maximum number of iterations exceeded
*gammcf = exp(-x + a * log(x) -(*gln)) * gold;
}
//returns the incomplete gamma function P(a,x)
double gammp(double a, double x)
{
double gamser, gammcf, gln;
if(x < 0.0 || a <= 0.0) return 0.0;
if(x < (a+1.0)) {
gser(&gamser, a, x, &gln); return gamser;
}
else {
gcf(&gammcf, a, x, &gln); return 1.0-gammcf;
}
return 0.0;
}
//returns the complementary incomplete gamma function Q(a,x)
double gammq(double a, double x)
{
double gamser, gammcf, gln;
if(x < 0.0 || a <= 0.0) return 0.0;
if(x < (a+1.0)) {
gser(&gamser, a, x, &gln); return 1.0-gamser;
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -