gronod/argyllcms
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 11 Wiki Activity
Files
development
argyllcms/numlib/dnsq.c
T
gronod 45bdf51201 Initial commit of v3.5.0
2026-08-20 20:28:40 +01:00

1677 lines
50 KiB
C
Raw Permalink Blame History

/* Concatenated dnsq code */
/*
* This concatenation, translation to C and modifications,
* Copyright 1998 Graeme Gill
* All rights reserved.
*
* This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :-
* see the License.txt file for licencing details.
*/
#include "numsup.h"
#include "dnsq.h" /* Public interface definitions */
#undef DEBUG
typedef long int abool;
#ifndef TRUE
# define FALSE (0)
# define TRUE (!FALSE)
#endif
#ifndef min
#define min(a,b) ((a) <= (b) ? (a) : (b))
#endif
#ifndef max
#define max(a,b) ((a) >= (b) ? (a) : (b))
#endif
#ifndef fabs
#define fabs(x) ((x) >= 0.0 ? (x) : -(x))
#endif
/* Forward difference jacobian approximation */
static int dfdjc1(
void *fdata,
int (*fcn)(void *fdata, int n, double *x, double *fvec, int iflag),
int n, double *x, double * fvec, double *fjac,
int ldfjac, int ml, int mu,
double epsfcn, double *wa1, double *wa2);
/* QR factorization */
static int dqrfac(int m, int n, double *a, int lda,
abool pivot, int *ipvt, double *sigma,
double *acnorm, double *wa);
/* Use QR decomposition to form the orthogonal matrix */
static int dqform(int m, int n, double *q, int ldq, double *wa);
/* QR decomposition update */
static int d1updt(int m, int n, double *s,
double *u, double *v, double *w);
/* Jacobian rotation, called by QR update */
static int d1mpyq(int m, int n, double *a, int lda,
double *v, double *w);
/* Compute norm of a vector */
static double denorm(int n, double *x);
/* Line search */
static int ddoglg(int n, double *r, double *diag, double *qtb,
double delta, double *x, double *wa1, double *wa2);
/***************************************************************/
/*
* A simplified interface to dnsq().
*/
int dnsqe(
void *fdata, /* Opaque pointer to pass to fcn() and jac() */
int (*fcn)(void *fdata, int n, double *x, double *fvec, int iflag),
/* Pointer to function we are solving */
int (*jac)(void *fdata, int n, double *x, double *fvec, double **fjac),
/* Optional function to compute jacobian, NULL if not used */
int n, /* Number of functions and variables */
double x[], /* Initial solution estimate, RETURNs final solution */
double ss, /* Initial search area */
double fvec[], /* Array that will be RETURNed with thefunction values at the solution */
double dtol, /* Desired delta tollerance of the solution */
double atol, /* Desired absolute tollerance of the solution */
int maxfev, /* Maximum number of function calls. set to 0 for automatic */
int nprint /* Turn on debugging printouts from func() every nprint itterations */
) {
int info = 0; /* Return status */
int nfev, njev;
int i,j, index, ml, lr, mu;
double epsfcn = ss * ss; /* Jacobian estimate step */
double factor = ss; /* Initial step size */
double maxstep = 0.0; /* Subsequent step size (not working ??) */
if (maxfev <= 0) {
maxfev = (n + 1) * 100;
if (jac == NULL)
maxfev <<= 1;
}
ml = n - 1; /* number of subdiagonals within the band of the Jacobian matrix. */
mu = n - 1; /* number of superdiagonals within the band of the Jacobian matrix. */
lr = n * (n + 1) / 2;
index = n * 6 + lr;
/* Call dnsq. */
info = dnsq(fdata, fcn, jac, NULL, 0,
n, &x[0], &fvec[0], dtol, atol,
maxfev, ml, mu, epsfcn, NULL, factor, maxstep, nprint,
&nfev, &njev);
if (info == 5)
info = 4;
if (info == 0)
warning("dnsqe: invalid input parameter.");
return info;
} /* dnsqe */
/***************************************************************/
/*
* Library: SLATEC
* Category: F2A
* Type: Double precision (SNSQE-S, DNSQE-D)
* Keywords easy-to-use, nonlinear square system,
* powell hybrid method, zeros
* Author: Hiebert, K. L. (SNLA)
* Translated to C by f2c and Graeme W. Gill
*
* 1. Purpose.
*
* The purpose of DNSQ is to find a zero of a system of N nonlinear
* functions in N variables by a modification of the Powell
* hybrid method. The user must provide a subroutine which
* calculates the functions. The user has the option of either to
* provide a subroutine which calculates the Jacobian or to let the
* code calculate it by a forward-difference approximation.
* This code is the combination of the MINPACK codes (Argonne)
* HYBRD and HYBRDJ.
*
* 2. Subroutine and Type Statements.
*
* SUBROUTINE DNSQ(FCN,JAC,N,X,FVEC,FJAC,LDFJAC,XTOL,MAXFEV,
* ML,MU,EPSFCN,DIAG,MODE,FACTOR,NPRINT,INFO,NFEV,
* NJEV,R,LR,QTF,WA1,WA2,WA3,WA4)
* INTEGER N,MAXFEV,ML,MU,MODE,NPRINT,INFO,NFEV,LDFJAC,NJEV,LR
* DOUBLE PRECISION XTOL,EPSFCN,FACTOR
* DOUBLE PRECISION
* X(N),FVEC(N),DIAG(N),FJAC(LDFJAC,N),R(LR),QTF(N),
* WA1(N),WA2(N),WA3(N),WA4(N)
* EXTERNAL FCN,JAC
*
* 3. Parameters.
*
* Parameters designated as input parameters must be specified on
* entry to DNSQ and are not changed on exit, while parameters
* designated as output parameters need not be specified on entry
* and are set to appropriate values on exit from DNSQ.
*
* fcn() is the name of the user-supplied subroutine which calculates
* the functions. fcn() must be declared in an external statement
* in the user calling program, and should be written as follows.
*
* int fcn(
* void *fdata,
* int n,
* double *x,
* double *fvec,
* int iflag);
* {
* calculate the functions at x and
* return this vector in fvec.
* print out vector if iflag == 0
* return 0 (or < 0 to abort)
* }
* The value 0 should be returned fcn() unless the
* user wants to terminate execution of DNSQ. In this case
* return a negative integer.
*
* jac() is the name of the user-supplied subroutine which calculates
* the Jacobian. If jac!=NULL, then jac() must be declared in an
* external statement in the user calling program, and should be
* written as follows.
*
* int jac(
* void *fdata,
* int n,
* double *x,
* double *fvec,
* double **fjac,
* {
* Calculate the Jacobian at x and return this
* matrix in fjac. fvec contains the function
* values at x and should not be altered.
* return 0 (or < 0 to abort)
* }
* The value 0 should be returned jac() unless the
* user wants to terminate execution of DNSQ. In this case
* return a negative integer.
*
* If jac == NULL, then the code will approximate the
* Jacobian by forward-differencing.
*
* double **sjac;
* sjac is an optional n by n matrix that can hold an initial
* jacobian matrix that will be used in preference to calling the jac()
* function, or to using forward differencing. If the array is provided,
* it will also contain the last jacobian matrix used before exiting.
* If this array is not used, it should be set to NULL.
*
* int startsjac;
* styatsjac is a flag, that when set to non-zero, will cause the
* sjac[] array to be used as the initial jacobian matrix, in preference
* to calling the jac() function, or to using forward differencing.
*
* n is a positive integer input variable set to the number of
* functions and variables.
*
* x is an array of length n. On input x must contain an initial
* estimate of the solution vector. On output x contains the
* final estimate of the solution vector.
*
* fvec is an output array of length n which contains the functions
* evaluated at the output x.
*
* fjac is an output N by N array which contains the orthogonal
* matrix Q produced by the QR factorization of the final
* approximate Jacobian.
*
* ldfjac is a positive integer input variable not less than n
* which specifies the leading dimension of the array fjac.
*
* dtol is a nonnegative input variable. Termination occurs when
* the relative error between two consecutive iterates is at most
* dtol. Therefore, dtol measures the relative error desired in
* the approximate solution. Section 4 contains more details
* about dtol.
*
* tol is a nonnegative input variable. Termination occurs when
* the value of all the function values falls below this threshold.
* Termination occurs when either the dtol or tol condition is met.
*
* maxfev is a positive integer input variable. Termination occurs
* when the number of calls to fcn is at least maxfev by the end
* of an iteration.
*
* ml is a nonnegative integer input variable which specifies the
* number of subdiagonals within the band of the Jacobian matrix.
* If the Jacobian is not banded or jac!=null, set ml to at
* least n - 1.
*
* mu is a nonnegative integer input variable which specifies the
* number of superdiagonals within the band of the Jacobian
* matrix. If the Jacobian is not banded or jac!=null, set mu to at
* least n - 1.
*
* epsfcn is an input variable used in determining a suitable step
* for the forward-difference approximation. This approximation
* assumes that the relative errors in the functions are of the
* order of epsfcn. If epsfcn is less than the machine
* precision, it is assumed that the relative errors in the
* functions are of the order of the machine precision. If
* jac!=null, then epsfcn can be ignored (treat it as a dummy
* argument).
*
* diag is an array of length n. If MODE = 1 (see below), diag is
* internally set. If mode = 2, diag must contain positive
* entries that serve as implicit (multiplicative) scale factors
* for the variables.
*
* mode is an integer input variable. If mode = 1, the variables
* will be scaled internally. If mode = 2, the scaling is
* specified by the input diag. Other values of mode are
* equivalent to mode = 1.
*
* factor is a positive input variable used in determining the
* initial step bound. This bound is set to the product of
* factor and the Euclidean norm of diag*x if nonzero, or else to
* factor itself. In most cases factor should lie in the
* interval (.1,100.). 100. is a generally recommended value.
*
* nprint is an integer input variable that enables controlled
* printing of iterates if it is positive. In this case, fcn is
* called with iflag = 0 at the beginning of the first iteration
* and every nprint iterations thereafter and immediately prior
* to return, with x and fvec available for printing. Appropriate
* print statements must be added to fcn(see example). If nprint
* is not positive, no special calls of fcn with iflag = 0 are
* made.
*
* info is an integer output variable. If the user has terminated
* execution, info is set to the (negative) value of iflag. See
* description of fcn and jac. Otherwise, INFO is set as follows.
*
* INFO = 0 Improper input parameters.
*
* INFO = 1 Relative error between two consecutive iterates is
* at most XTOL. Normal sucessful return value.
*
* INFO = 2 Number of calls to FCN has reached or exceeded
* MAXFEV.
*
* INFO = 3 XTOL is too small. No further improvement in the
* approximate solution X is possible.
*
* INFO = 4 Iteration is not making good progress, as measured
* by the improvement from the last five Jacobian
* evaluations.
*
* INFO = 5 Iteration is not making good progress, as measured
* by the improvement from the last ten iterations.
* Return value if no zero can be found from this starting
* point.
*
* Sections 4 and 5 contain more details about INFO.
*
* nfev is an integer output variable set to the number of calls to
* fcn.
*
* njev is an integer output variable set to the number of calls to
* jac. (If jac==null, then njev is set to zero.)
*
* 4. Successful completion.
*
* The accuracy of DNSQ is controlled by the convergence parameter
* XTOL. This parameter is used in a test which makes a comparison
* between the approximation X and a solution XSOL. DNSQ
* terminates when the test is satisfied. If the convergence
* parameter is less than the machine precision (as defined by the
* function D1MACH(4)), then DNSQ only attempts to satisfy the test
* defined by the machine precision. Further progress is not
* usually possible.
*
* The test assumes that the functions are reasonably well behaved,
* and, if the Jacobian is supplied by the user, that the functions
* and the Jacobian are coded consistently. If these conditions
* are not satisfied, then DNSQ may incorrectly indicate
* convergence. The coding of the Jacobian can be checked by the
* subroutine DCKDER. If the Jacobian is coded correctly or JAC==NULL,
* then the validity of the answer can be checked, for example, by
* rerunning DNSQ with a tighter tolerance.
*
* Convergence Test. If DENORM(Z) denotes the Euclidean norm of a
* vector Z and D is the diagonal matrix whose entries are
* defined by the array DIAG, then this test attempts to
* guarantee that
*
* DENORM(D*(X-XSOL)) .LE. XTOL*DENORM(D*XSOL).
*
* If this condition is satisfied with XTOL = 10**(-K), then the
* larger components of D*X have K significant decimal digits and
* INFO is set to 1. There is a danger that the smaller
* components of D*X may have large relative errors, but the fast
* rate of convergence of DNSQ usually avoids this possibility.
*
* Unless high precision solutions are required, the recommended
* value for XTOL is the square root of the machine precision.
*
*
* 5. Unsuccessful Completion.
*
* Unsuccessful termination of DNSQ can be due to improper input
* parameters, arithmetic interrupts, an excessive number of
* function evaluations, or lack of good progress.
*
* Improper Input Parameters. INFO is set to 0 if
* N .LE. 0, or LDFJAC .LT. N, or
* XTOL .LT. 0.E0, or MAXFEV .LE. 0, or ML .LT. 0, or MU .LT. 0,
* or FACTOR .LE. 0.E0, or LR .LT. (N*(N+1))/2.
*
* Arithmetic Interrupts. If these interrupts occur in the FCN
* subroutine during an early stage of the computation, they may
* be caused by an unacceptable choice of X by DNSQ. In this
* case, it may be possible to remedy the situation by rerunning
* DNSQ with a smaller value of FACTOR.
*
* Excessive Number of Function Evaluations. A reasonable value
* for MAXFEV is 100*(N+1) for JAC!=NULL and 200*(N+1) for JAC==NULL.
*
* If the number of calls to FCN reaches MAXFEV, then this
* indicates that the routine is converging very slowly as
* measured by the progress of FVEC, and INFO is set to 2. This
*
* situation should be unusual because, as indicated below, lack
* of good progress is usually diagnosed earlier by DNSQ,
* causing termination with info = 4 or INFO = 5.
*
* Lack of Good Progress. DNSQ searches for a zero of the system
*
* by minimizing the sum of the squares of the functions. In so
* doing, it can become trapped in a region where the minimum
* does not correspond to a zero of the system and, in this
* situation, the iteration eventually fails to make good
* progress. In particular, this will happen if the system does
* not have a zero. If the system has a zero, rerunning DNSQ
* from a different starting point may be helpful.
*
*
* 6. Characteristics of The Algorithm.
*
* DNSQ is a modification of the Powell Hybrid method. Two of its
* main characteristics involve the choice of the correction as a
* convex combination of the Newton and scaled gradient directions,
* and the updating of the Jacobian by the rank-1 method of
* Broyden. The choice of the correction guarantees (under
* reasonable conditions) global convergence for starting points
* far from the solution and a fast rate of convergence. The
* Jacobian is calculated at the starting point by either the
* user-supplied subroutine or a forward-difference approximation,
* but it is not recalculated until the rank-1 method fails to
* produce satisfactory progress.
*
* Timing. The time required by DNSQ to solve a given problem
* depends on N, the behavior of the functions, the accuracy
* requested, and the starting point. The number of arithmetic
* operations needed by DNSQ is about 11.5*(N**2) to process
* each evaluation of the functions (call to FCN) and 1.3*(N**3)
* to process each evaluation of the Jacobian (call to JAC,
* if JAC!=NULL). Unless FCN and JAC can be evaluated quickly,
* the timing of DNSQ will be strongly influenced by the time
* spent in FCN and JAC.
*
* Storage. DNSQ requires (3*N**2 + 17*N)/2 single precision
* storage locations, in addition to the storage required by the
* program. There are no internally declared storage arrays.
*
* References: M. J. D. Powell, A hybrid method for nonlinear equa-
* tions. In Numerical Methods for Nonlinear Algebraic
* Equations, P. Rabinowitz, Editor. Gordon and Breach,
* 1988.
*
* Routines called: D1MPYQ, D1UPDT, DDOGLG, DENORM, DFDJC1, DQFORM, DQRFAC
*
* Revision history: (YYMMDD)
* 800301 DATE WRITTEN
* 890531 Changed all specific intrinsics to generic. (WRB)
* 890831 Modified array declarations. (WRB)
* 890831 REVISION DATE from Version 3.2
* 891214 Prologue converted to Version 4.0 format. (BAB)
* 900315 CALLs to XERROR changed to CALLs to XERMSG. (THJ)
* 920501 Reformatted the REFERENCES section. (WRB)
* 960510 Translated to C and features added. (GWG)
*/
/* Returns status. 0 = OK. */
int dnsq(
void *fdata, /* Opaque data pointer, passed to called functions */
int (*fcn)(void *fdata, int n, double *x, double *fvec, int iflag),
/* Pointer to function we are solving */
int (*jac)(void *fdata, int n, double *x, double *fvec, double **fjac),
/* Optional function to compute jacobian, NULL if not used */
double **sjac, /* Optional initial jacobian matrix/last jacobian matrix. NULL if not used.*/
int startsjac, /* Set to non-zero to use sjac as the initial jacobian */
int n, /* Number of functions and variables */
double x[], /* Initial solution estimate, RETURNs final solution */
double fvec[], /* Array that will be RETURNed with thefunction values at the solution */
double dtol, /* Desired delta tollerance of the solution */
double atol, /* Desired tollerance of the root (stops on dtol or tol) */
int maxfev, /* Set excessive Number of Function Evaluations */
int ml, /* number of subdiagonals within the band of the Jacobian matrix. */
int mu, /* number of superdiagonals within the band of the Jacobian matrix. */
double epsfcn, /* determines suitable step for forward-difference approximation */
double diag[], /* Optional scaling factors, use NULL for internal scaling */
double factor, /* Determines the initial step bound */
double maxstep, /* Determines the maximum subsequent step bound (0.0 for none) */
/* maxstep is NOT WORKING !!! ??? */
int nprint, /* Turn on debugging printouts from func() every nprint itterations */
int *nfev, /* RETURNs the number of calls to fcn() */
int *njev /* RETURNs the number of calls to jac() */
) {
int info = 0; /* Return status - invalid argument */
int smode = 0; /* Scaling mode, 1 = internal */
/* Internal working arrays */
double *fjac = NULL; /* n by n array which contains the orthogonal matrix Q */
/* produced by the QR factorization of the final approximate Jacobian. */
int ldfjac = n; /* stride of 2d array */
double **jjac = NULL; /* NR style pointers to fjac */
double *r = NULL; /* Array of length (n*(n+1))/2 which contains the upper */
/* triangular matrix produced by the QR factorization of the */
/* final approximate Jacobian, stored rowwise. */
double *qtf = NULL; /* Array of length n which contains the vector (q transpose)*fvec. */
double *wa1 = NULL; /* Four working arrays length n */
double *wa2 = NULL;
double *wa3 = NULL;
double *wa4 = NULL;
int iwa[1]; /* Pivot swap array (only one element used) */
/* Local variables */
abool jeval;
int iter;
int i, j, k, l, iflag;
int qrflag; /* Set when a valid Q is in fjac[], and R is in r[] */
int ncsuc;
int nslow1, nslow2, ncfail;
double temp;
double delta = 0.0;
double ratio, fnorm, pnorm;
double xnorm = 0.0;
double fnorm1;
double actred, prered;
double sum;
/* Allocate out working arrays */
if (diag == NULL) { /* Internal scaling */
smode = 1;
diag = dvector(0,n-1);
}
fjac = dvector(0,(n * n)-1);
jjac = convert_dmatrix(fjac,0,n-1,0,ldfjac-1);
r = dvector(0,((n * (n+1))/2)-1);
qtf = dvector(0,n-1);
wa1 = dvector(0,n-1);
wa2 = dvector(0,n-1);
wa3 = dvector(0,n-1);
wa4 = dvector(0,n-1);
qrflag = 0;
iflag = 0;
*nfev = 0;
*njev = 0;
/* Check the input parameters for errors. */
if (n <= 0 || dtol < 0.0 || maxfev <= 0
|| ml < 0 || mu < 0 || factor <= 0.0 || maxstep < 0.0
|| (sjac == NULL && startsjac != 0)) {
goto func_exit;
}
if (!smode) { /* Check the scaling array we were given */
for (j = 0; j < n; ++j) {
if (diag[j] <= 0.0) {
goto func_exit;
}
}
}
/* Evaluate the function at the starting point */
/* and calculate its norm. */
*nfev = 1;
if ((iflag = (*fcn)(fdata, n, &x[0], &fvec[0], 1)) < 0)
goto func_exit;
fnorm = denorm(n, &fvec[0]);
/* Initialize iteration counter and monitors. */
iter = 1;
ncsuc = 0;
ncfail = 0;
nslow1 = 0;
nslow2 = 0;
/* Beginning of the outer loop. */
for (;;) {
jeval = TRUE;
/* initialize the jacobian matrix. */
if (startsjac) { /* User provided array */
for (j = 0; j < n; j++) {
for (i = 0; i < n; i++)
fjac[j * ldfjac + i] = sjac[j][i];
}
} else if (jac == NULL) { /* Code approximates the jacobian */
int ti;
iflag = dfdjc1(fdata, fcn, n, &x[0], &fvec[0], &fjac[0], ldfjac,
ml, mu, epsfcn, &wa1[0], &wa2[0]);
ti = ml + mu + 1;
*nfev += min(ti,n);
} else { /* User supplies jacobian function */
iflag = (*jac)(fdata, n, &x[0], &fvec[0], jjac);
++(*njev);
}
#ifdef DEBUG
printf("DNSQ: Jacobian initialized\n");
#endif
if (iflag < 0)
goto func_exit;
/* Compute the qr factorization of the jacobian. */
dqrfac(n, n, &fjac[0], ldfjac, FALSE, iwa, &wa1[0], &wa2[0], &wa3[0]);
/* On the first iteration and if internal scaling mode set, scale */
/* according to the norms of the columns of the initial jacobian. */
/* (wa2[] will contain norms) */
/* Do this on subsequent itterations too, if a maxstep is set. */
if (iter == 1 || maxstep > 0.0) {
if (smode) {
for (j = 0; j < n; ++j) {
diag[j] = wa2[j];
if (wa2[j] == 0.0) {
diag[j] = 1.0;
}
}
}
/* On the first iteration, calculate the norm of the scaled */
/* x[], and initialize the step bound delta. */
for (j = 0; j < n; ++j)
wa3[j] = diag[j] * x[j];
xnorm = denorm(n, &wa3[0]);
if (iter == 1) {
delta = factor * xnorm;
if (delta == 0.0)
delta = factor;
#ifdef DEBUG
printf("Initial Delta = %f\n",delta);
#endif /* DEBUG */
} else {
delta = maxstep * xnorm;
if (delta == 0.0)
delta = maxstep;
#ifdef DEBUG
printf("Subsequent Delta = %f\n",delta);
#endif /* DEBUG */
}
}
/* Form (q transpose)*fvec and store in qtf. */
for (i = 0; i < n; ++i)
qtf[i] = fvec[i];
for (j = 0; j < n; ++j) {
if (fjac[j + j * ldfjac] != 0.0) {
sum = 0.0;
for (i = j; i < n; ++i)
sum += fjac[i + j * ldfjac] * qtf[i];
temp = -sum / fjac[j + j * ldfjac];
for (i = j; i < n; ++i)
qtf[i] += fjac[i + j * ldfjac] * temp;
}
}
/* Copy the triangular factor of the qr factorization into r. */
for (j = 0; j < n; ++j) {
l = j;
if (j >= 1) {
for (i = 0; i < j; ++i) {
r[l] = fjac[i + j * ldfjac];
l += (n-1) - i;
}
}
r[l] = wa1[j];
}
/* Accumulate the orthogonal factor Q in fjac. */
dqform(n, n, &fjac[0], ldfjac, &wa1[0]);
qrflag = 1;
/* Rescale if necessary. */
if (smode) {
for (j = 0; j < n; ++j) {
diag[j] = max(diag[j],wa2[j]);
}
}
/* Beginning of the inner loop. */
for (;;) {
/* If requested, call fcn to enable printing of iterates. */
if (nprint > 0) {
if ((iter - 1) % nprint == 0)
if ((iflag = (*fcn)(fdata, n, &x[0], &fvec[0], 0)) < 0)
goto func_exit;
}
#ifdef DEBUG
/* If the user supplied an array, and there is a valid Q in */
/* fjac[], and R is in r[], recover the most recent Jacobian */
/* matrix by multiplying Q by R */
if (qrflag && sjac) {
for (i = 0; i < n; ++i) {
for (j = 0; j < n; ++j) {
double temp = 0.0;
l = j;
for (k = 0; k <= j; ++k) {
temp += fjac[k * ldfjac + i] * r[l];
l += (n-1-k);
}
sjac[j][i] = temp;
}
}
printf("Current Jacobian = \n");
for (j = 0; j < n; ++j) {
for (i = 0; i < n; ++i) {
printf("%f ",sjac[j][i]);
}
printf("\n");
}
}
#endif /* DEBUG */
/* Determine the direction p. */
ddoglg(n, &r[0], &diag[0], &qtf[0], delta, &wa1[0], &wa2[0], &wa3[0]);
/* Store the direction p and x + p. calculate the norm of p. */
/* (wa1[] is X[] output array from ddoglg()) */
for (j = 0; j < n; ++j) {
wa1[j] = -wa1[j];
wa2[j] = x[j] + wa1[j];
wa3[j] = diag[j] * wa1[j];
}
pnorm = denorm(n, &wa3[0]);
/* On the first iteration, adjust the initial step bound. */
/* Do this on subsequent itterations too, if maxstep is set. */
if (iter == 1 || maxstep > 0.0) {
delta = min(delta,pnorm);
#ifdef DEBUG
if (iter == 1)
printf("First itter Delta = %f\n",delta);
else
printf("Subsequent itter Delta = %f\n",delta);
#endif /* DEBUG */
}
/* Evaluate the function at x + p and calculate its norm. */
++(*nfev);
if ((iflag = (*fcn)(fdata, n, &wa2[0], &wa4[0], 1)) < 0)
goto func_exit;
fnorm1 = denorm(n, &wa4[0]);
/* Compute the scaled actual reduction. */
actred = -1.0; /* Assume norm is made worse */
if (fnorm1 < fnorm) { /* There was a reduction in the norm */
double td;
td = fnorm1 / fnorm;
actred = 1.0 - td * td;
}
/* Compute the scaled predicted reduction. */
l = 0;
for (i = 0; i < n; ++i) {
sum = 0.0;
for (j = i; j < n; ++j) {
sum += r[l] * wa1[j];
++l;
}
wa3[i] = qtf[i] + sum;
}
temp = denorm(n, &wa3[0]);
prered = 0.0;
if (temp < fnorm) {
double td;
td = temp / fnorm;
prered = 1.0 - td * td;
}
/* Compute the ratio of the actual to the predicted reduction. */
ratio = 0.0; /* Assume no improvement */
if (prered > 0.0)
ratio = actred / prered;
#ifdef DEBUG
printf("DNSQ: actual/predicted ratio = %f\n",ratio);
#endif /* DEBUG */
/* Update the step bound. */
if (ratio < 0.1) {
ncsuc = 0;
++ncfail; /* Forces jacobian recalc when ncfail == 2 */
delta = 0.5 * delta;
#ifdef DEBUG
printf("ratio < 0.1 bad, Delta = %f, ncfail = %d\n",delta,ncfail);
#endif /* DEBUG */
} else {
ncfail = 0; /* Pospone jacobian recalc */
++ncsuc;
if (ratio >= 0.5 || ncsuc > 1) {
delta = max(delta, pnorm / 0.5);
#ifdef DEBUG
printf("ratio > 0.1 good, Delta = %f, ncsuc = %d\n",delta,ncsuc);
#endif /* DEBUG */
}
if (fabs(ratio - 1.0) <= 0.1) {
delta = 2.0 * pnorm;
#ifdef DEBUG
printf("abs(ratio -1.0) <= 0.1 Delta = %f\n",delta);
#endif /* DEBUG */
}
}
/* Test for progressing iteration. */
if (ratio > 0.0001) {
#ifdef DEBUG
printf("Successful itter\n");
#endif /* DEBUG */
/* Successful iteration. update x, fvec, and their norms. */
for (j = 0; j < n; ++j) {
x[j] = wa2[j];
wa2[j] = diag[j] * x[j];
fvec[j] = wa4[j];
}
xnorm = denorm(n, &wa2[0]);
fnorm = fnorm1;
++iter;
}
#ifdef DEBUG
printf("DNSQ: actual = %f\n",actred);
#endif /* DEBUG */
/* Determine the progress of the iteration. */
++nslow1;
if (fabs(actred) >= 0.001)
nslow1 = 0;
if (jeval)
++nslow2;
if (fabs(actred) >= 0.1)
nslow2 = 0;
/* Test for convergence. */
if (delta <= dtol * xnorm || fnorm == 0.0) {
#ifdef DEBUG
printf("DNSQ: delta %f <= dtol * xnorm %f || fnorm == %f\n",delta,dtol * xnorm,fnorm);
#endif /* DEBUG */
info = 1;
goto func_exit;
}
/* Test for root meeting tolerance (GWG) */
for (j = 0; j < n; ++j) {
if (fabs(fvec[j]) > atol)
break;
}
if (j >= n) { /* All were below tollerance */
#ifdef DEBUG
printf("DNSQ: fvecs are all <= atol %f\n",atol);
#endif /* DEBUG */
info = 1;
goto func_exit;
}
/* Tests for termination and stringent tolerances. */
if (*nfev >= maxfev)
info = 2;
if (0.1 * max(0.1 * delta, pnorm) <= M_DIVER * xnorm)
info = 3;
if (nslow2 == 5)
info = 4;
if (nslow1 == 10)
info = 5;
if (info != 0)
goto func_exit;
/* Criterion for recalculating jacobian */
if (ncfail == 2) {
break; /* Break out of inner loop */
}
/* Calculate the rank one modification to the jacobian */
/* and update qtf if necessary. */
for (j = 0; j < n; ++j) {
sum = 0.0;
for (i = 0; i < n; ++i) {
sum += fjac[i + j * ldfjac] * wa4[i];
}
wa2[j] = (sum - wa3[j]) / pnorm;
wa1[j] = diag[j] * (diag[j] * wa1[j] / pnorm);
if (ratio >= 1e-4) {
qtf[j] = sum;
}
}
/* Compute the qr factorization of the updated jacobian. */
d1updt(n, n, &r[0], &wa1[0], &wa2[0], &wa3[0]);
d1mpyq(n, n, &fjac[0], ldfjac, &wa2[0], &wa3[0]);
d1mpyq(1, n, &qtf[0], 1, &wa2[0], &wa3[0]);
jeval = FALSE;
} /* End of the inner loop. */
} /* End of the outer loop. */
/* Termination, either normal or user imposed. */
func_exit:
/* If the user supplied an array, and there is a valid Q in */
/* fjac[], and R is in r[], recover the most recent Jacobian */
/* matrix by multiplying Q by R */
if (qrflag && sjac) {
for (i = 0; i < n; ++i) {
for (j = 0; j < n; ++j) {
double temp = 0.0;
l = j;
for (k = 0; k <= j; ++k) {
temp += fjac[k * ldfjac + i] * r[l];
l += (n-1-k);
}
sjac[j][i] = temp;
}
}
}
/* Free our working arrays */
if (smode)
free_dvector(diag,0,n-1);
free_dvector(fjac,0,(n * n)-1);
free_convert_dmatrix(jjac,0,n-1,0,ldfjac-1);
free_dvector(r,0,((n * (n+1))/2)-1);
free_dvector(qtf,0,n-1);
free_dvector(wa1,0,n-1);
free_dvector(wa2,0,n-1);
free_dvector(wa3,0,n-1);
free_dvector(wa4,0,n-1);
if (iflag < 0)
info = iflag;
if (nprint > 0)
(*fcn)(fdata, n, &x[0], &fvec[0], 0);
#ifdef DEBUG
if (info < 0)
printf("dnsq: Execution terminated because user set iflag negative\n");
if (info == 0)
printf("dnsq: Invalid input parameter\n");
if (info == 2)
printf("dnsq: Too many function evaluations\n");
if (info == 3)
printf("dnsq: dtol too small. no further improvement possible\n");
if (info > 4)
printf("dnsq: Iteration not making good progress\n");
#endif /* DEBUG */
return info;
} /* dnsq */
/***************************************************************/
/***************************************************************/
/*
* Given an M by N matrix A, this subroutine computes A*Q where
* Q is the product of 2*(N - 1) transformations
*
* GV(N-1)*...*GV(1)*GW(1)*...*GW(N-1)
*
* and GV(I), GW(I) are Givens rotations in the (I,N) plane which
* eliminate elements in the I-th and N-th planes, respectively.
* Q itself is not given, rather the information to recover the
* GV, GW rotations is supplied.
*
*/
static int d1mpyq(
int m, /* Number of rows of A */
int n, /* Number of columns of A */
double *a, /* M by N array */
int lda, /* stride for a[][] */
double *v, /* Input array */
double *w) /* Input array */
{
/* Local variables */
int i, j;
int nm1 = n - 1;
double temp;
double cos_ = 0.0, sin_ = 0.0;
/* Apply the first set of givens rotations to a. */
if (nm1 >= 1) {
for (j = n-2; j >= 0; --j) {
if (fabs(v[j]) > 1.0)
cos_ = 1.0 / v[j];
if (fabs(v[j]) > 1.0) { /* Computing 2nd power */
sin_ = sqrt(1.0 - cos_ * cos_);
}
if (fabs(v[j]) <= 1.0) {
sin_ = v[j];
}
if (fabs(v[j]) <= 1.0) { /* Computing 2nd power */
cos_ = sqrt(1.0 - sin_ * sin_);
}
for (i = 0; i < m; ++i) {
temp = cos_ * a[i + j * lda] - sin_ * a[i + nm1 * lda];
a[i + nm1 * lda] = sin_ * a[i + j * lda] + cos_ * a[i + nm1 * lda];
a[i + j * lda] = temp;
}
}
/* Apply the second set of givens rotations to a. */
for (j = 0; j < nm1; ++j) {
if (fabs(w[j]) > 1.0)
cos_ = 1.0 / w[j];
if (fabs(w[j]) > 1.0) { /* Computing 2nd power */
sin_ = sqrt(1.0 - cos_ * cos_);
}
if (fabs(w[j]) <= 1.0)
sin_ = w[j];
if (fabs(w[j]) <= 1.0) { /* Computing 2nd power */
cos_ = sqrt(1.0 - sin_ * sin_);
}
for (i = 0; i < m; ++i) {
temp = cos_ * a[i + j * lda] + sin_ * a[i + nm1 * lda];
a[i + nm1 * lda] = -sin_ * a[i + j * lda] + cos_ * a[i + nm1 * lda];
a[i + j * lda] = temp;
}
}
}
return 0;
}
/***************************************************************/
/***************************************************************/
/*
* Given an M by N lower trapezoidal matrix S, an M-vector U,
* and an N-vector V, the problem is to determine an
* orthogonal matrix Q such that
*
* t
* (S + U*V )*Q
*
* is again lower trapezoidal.
*
* This subroutine determines Q as the product of 2*(N - 1)
* transformations
*
* GV(N-1)*...*GV(1)*GW(1)*...*GW(N-1)
*
* where GV(I), GW(I) are Givens rotations in the (I,N) plane
* which eliminate elements in the I-th and N-th planes,
* respectively. Q itself is not accumulated, rather the
* information to recover the GV, GW rotations is returned.
*
*/
static int d1updt(
int m, /* Number of rows of S */
int n, /* Number of columns of S */
double *s, /* Array of length (n*(2*m-n+1))/2 containing the lower trapezoid matrix */
double *u, /* U vector lenghth m */
double *v, /* V vector length n */
double *w) /* Output array W length m */
{
int i, j, l;
int jj;
int nm1 = n - 1;
int nmj;
double temp;
double giant, cotan;
double tan_;
double cos_, sin_, tau;
/* Giant is the largest magnitude. */
giant = M_LARGE;
/* Initialize the diagonal element pointer. */
jj = n * ((m << 1) - n + 1) / 2 - (m - n) - 1;
/* Move the nontrivial part of the last column of s into w. */
l = jj;
for (i = nm1; i < m; ++i) {
w[i] = s[l];
++l;
}
/* Rotate the vector v into a multiple of the n-th unit vector */
/* in such a way that a spike is introduced into w. */
if (nm1 >= 1) {
for (nmj = 1; nmj <= nm1; ++nmj) {
j = n - nmj - 1;
jj -= m - j;
w[j] = 0.0;
if (v[j] == 0.0)
continue;
/* Determine a givens rotation which eliminates the */
/* j-th element of v. */
if (fabs(v[nm1]) < fabs(v[j])) {
cotan = v[nm1] / v[j];
sin_ = 0.5 / sqrt(0.25 + 0.25 * (cotan * cotan));
cos_ = sin_ * cotan;
tau = 1.0;
if (fabs(cos_) * giant > 1.0) {
tau = 1.0 / cos_;
}
} else {
tan_ = v[j] / v[nm1];
cos_ = 0.5 / sqrt(0.25 + 0.25 * (tan_ * tan_));
sin_ = cos_ * tan_;
tau = sin_;
}
/* Apply the transformation to v and store the information */
/* necessary to recover the givens rotation. */
v[nm1] = sin_ * v[j] + cos_ * v[nm1];
v[j] = tau;
/* Apply the transformation to s and extend the spike in w. */
l = jj;
for (i = j; i < m; ++i) {
temp = cos_ * s[l] - sin_ * w[i];
w[i] = sin_ * s[l] + cos_ * w[i];
s[l] = temp;
++l;
}
}
}
/* Add the spike from the rank 1 update to w. */
for (i = 0; i < m; ++i)
w[i] += v[nm1] * u[i];
/* Eliminate the spike. */
if (nm1 >= 1) {
for (j = 0; j < nm1; ++j) {
if (w[j] != 0.0) {
/* Determine a givens rotation which eliminates the */
/* j-th element of the spike. */
if (fabs(s[jj]) < fabs(w[j])) {
cotan = s[jj] / w[j];
sin_ = 0.5 / sqrt(0.25 + 0.25 * (cotan * cotan));
cos_ = sin_ * cotan;
tau = 1.0;
if (fabs(cos_) * giant > 1.0) {
tau = 1.0 / cos_;
}
} else {
tan_ = w[j] / s[jj];
cos_ = 0.5 / sqrt(0.25 + 0.25 * (tan_ * tan_));
sin_ = cos_ * tan_;
tau = sin_;
}
/* Apply the transformation to s and reduce the spike in w. */
l = jj;
for (i = j; i < m; ++i) {
temp = cos_ * s[l] + sin_ * w[i];
w[i] = -sin_ * s[l] + cos_ * w[i];
s[l] = temp;
++l;
}
/* Store the information necessary to recover the givens rotation. */
w[j] = tau;
}
jj += m - j;
}
}
/* Move w back into the last column of the output s. */
l = jj;
for (i = nm1; i < m; ++i) {
s[l] = w[i];
++l;
}
return 0;
}
/***************************************************************/
/***************************************************************/
/*
* Given an M by N matrix A, an N by N nonsingular diagonal
* matrix D, an M-vector B, and a positive number DELTA, the
* problem is to determine the convex combination X of the
* Gauss-Newton and scaled gradient directions that minimizes
* (A*X - B) in the least squares sense, subject to the
* restriction that the Euclidean norm of D*X be at most DELTA.
*
* This subroutine completes the solution of the problem
* if it is provided with the necessary information from the
* QR factorization of A. That is, if A = Q*R, where Q has
* orthogonal columns and R is an upper triangular matrix,
* then DDOGLG expects the full upper triangle of R and
* the first N components of (Q transpose)*B.
*
*/
static int ddoglg(
int n, /* Order of r[] */
double r[], /* Array of length (n*(n+!))/2 containing upper triangular matrix */
double diag[], /* Array of length n containing the diagonal elements of the matrix d[] */
double qtb[], /* Array of length n containing first n elements of vector (Q transpose)*B */
double delta, /* Upper bound on the Euclidean norm of D*X */
double x[], /* output array of length N containing the desired direction */
double wa1[], /* Working arrays length n */
double wa2[])
{
int i, j, k, l;
int jj;
int jp1;
int nm1 = n-1;
double temp;
double alpha, gnorm, qnorm;
double epsmch;
double sgnorm;
double sum;
/* Epsmch is the machine precision. */
epsmch = M_DIVER;
/* First, calculate the gauss-newton direction. */
jj = n * (n + 1) / 2;
for (k = 0; k < n; ++k) {
j = nm1 - k;
jp1 = j + 1;
jj -= (k+1);
l = jj + 1;
sum = 0.0;
if (n >= (jp1+1)) {
for (i = jp1; i < n; ++i) {
sum += r[l] * x[i];
++l;
}
}
temp = r[jj];
if (temp == 0.0) {
l = j;
for (i = 0; i <= j; ++i) { /* Computing MAX */
double dt;
dt = fabs(r[l]);
temp = max(temp,dt);
l += nm1 - i;
}
temp = epsmch * temp;
if (temp == 0.0) {
temp = epsmch;
}
}
x[j] = (qtb[j] - sum) / temp;
}
/* Test whether the gauss-newton direction is acceptable. */
for (j = 0; j < n; ++j) {
wa1[j] = 0.0;
wa2[j] = diag[j] * x[j];
}
qnorm = denorm(n, &wa2[0]);
if (qnorm <= delta) {
return 0; /* Done - use this direction */
}
/* The gauss-newton direction is not acceptable. */
/* Next, calculate the scaled gradient direction. */
l = 0;
for (j = 0; j < n; ++j) {
temp = qtb[j];
for (i = j; i < n; ++i) {
wa1[i] += r[l] * temp;
++l;
}
wa1[j] /= diag[j];
}
/* Calculate the norm of the scaled gradient and test for */
/* The special case in which the scaled gradient is zero. */
gnorm = denorm(n, &wa1[0]);
sgnorm = 0.0;
alpha = delta / qnorm;
if (gnorm != 0.0) {
/* Calculate the point along the scaled gradient */
/* at which the quadratic is minimized. */
for (j = 0; j < n; ++j)
wa1[j] = wa1[j] / gnorm / diag[j];
l = 0;
for (j = 0; j < n; ++j) {
sum = 0.0;
for (i = j; i < n; ++i) {
sum += r[l] * wa1[i];
++l;
}
wa2[j] = sum;
}
temp = denorm(n, &wa2[0]);
sgnorm = gnorm / temp / temp;
/* Test whether the scaled gradient direction is acceptable. */
alpha = 0.0;
if (sgnorm < delta) {
double d0,d1,d2,d3,d4,
/* The scaled gradient direction is not acceptable. */
/* Finally, calculate the point along the dogleg */
/* at which the quadratic is minimized. */
bnorm = denorm(n, &qtb[0]);
d0 = bnorm / gnorm * (bnorm / qnorm) * (sgnorm / delta);
d1 = sgnorm / delta;
d2 = d0 - delta / qnorm;
d3 = delta / qnorm;
d4 = sgnorm / delta;
d0 = d0 - delta / qnorm * (d1 * d1)
+ sqrt(d2 * d2 + (1.0 - d3 * d3) * (1.0 - d4 * d4));
d1 = sgnorm / delta;
alpha = delta / qnorm * (1.0 - d1 * d1) / d0;
}
}
/* Form appropriate convex combination of the gauss-newton */
/* direction and the scaled gradient direction. */
temp = (1.0 - alpha) * min(sgnorm,delta);
for (j = 0; j < n; ++j)
x[j] = temp * wa1[j] + alpha * x[j];
return 0;
} /* ddoglg */
/***************************************************************/
/***************************************************************/
/*
* Given an N-vector X, this function calculates the
* Euclidean norm of X.
*
* The Euclidean norm is computed by accumulating the sum of
* squares in three different sums. The sums of squares for the
* small and large components are scaled so that no overflows
* occur. Non-destructive underflows are permitted. Underflows
* and overflows do not occur in the computation of the unscaled
* sum of squares for the intermediate components.
* The definitions of small, intermediate and large components
* depend on two constants, RDWARF and RGIANT. The main
* restrictions on these constants are that RDWARF**2 not
* underflow and RGIANT**2 not overflow. The constants
* given here are suitable for every known computer.
*
*/
static double denorm(
int n, /* Size of x[] */
double x[]) /* Input vector */
{
if (n < 8) { /* Make it simple and fast */
double ss = 0.0;
switch (n) {
case 8:
ss += x[7] * x[7];
case 7:
ss += x[6] * x[6];
case 6:
ss += x[5] * x[5];
case 5:
ss += x[4] * x[4];
case 4:
ss += x[3] * x[3];
case 3:
ss += x[2] * x[2];
case 2:
ss += x[1] * x[1];
case 1:
ss += x[0] * x[0];
}
return sqrt(ss);
} else {
/* Initialized data */
static double rdwarf = 3.834e-20;
static double rgiant = 1.304e19;
/* Local variables */
static double xabs, x1max, x3max;
static int i;
static double s1, s2, s3, agiant, floatn;
double ret_val, td;
s1 = 0.0; /* Large component */
s2 = 0.0; /* Intermedate component */
s3 = 0.0; /* Small component */
x1max = 0.0;
x3max = 0.0;
floatn = (double) (n + 1);
agiant = rgiant / floatn;
for (i = 0; i < n; i++) {
xabs = (td = x[i], fabs(td));
/* Sum for intermediate components. */
if (xabs > rdwarf && xabs < agiant) {
td = xabs; /* Computing 2nd power */
s2 += td * td;
/* Sum for small components. */
} else if (xabs <= rdwarf) {
if (xabs <= x3max) {
if (xabs != 0.0) { /* Computing 2nd power */
td = xabs / x3max;
s3 += td * td;
}
} else { /* Computing 2nd power */
td = x3max / xabs;
s3 = 1.0 + s3 * (td * td);
x3max = xabs;
}
/* Sum for large components. */
} else {
if (xabs <= x1max) { /* Computing 2nd power */
td = xabs / x1max;
s1 += td * td;
} else { /* Computing 2nd power */
td = x1max / xabs;
s1 = 1.0 + s1 * (td * td);
x1max = xabs;
}
}
}
/* Calculation of norm. */
if (s1 != 0.0) { /* Large is present */
ret_val = x1max * sqrt(s1 + s2 / x1max / x1max);
} else { /* Medium and small are present */
if (s2 == 0.0) {
ret_val = x3max * sqrt(s3); /* Small only */
} else {
if (s2 >= x3max) { /* Medium larger than small */
ret_val = sqrt(s2 * (1.0 + x3max / s2 * (x3max * s3)));
} else { /* Small large than medium */
ret_val = sqrt(x3max * (s2 / x3max + x3max * s3));
}
}
}
return ret_val;
}
}
/***************************************************************/
/***************************************************************/
/*
* This subroutine computes a forward-difference approximation
* to the N by N Jacobian matrix associated with a specified
* problem of N functions in N variables. If the Jacobian has
* a banded form, then function evaluations are saved by only
* approximating the nonzero terms.
*
*/
static int dfdjc1( /* Return < 0 if fcn() aborts */
void *fdata, /* Opaque data pointer to pass to fcn() */
int (*fcn)(void *fdata, int n, double *x, double *fvec, int iflag),
/* Pointer to function we are solving */
int n, /* Number of functions and variables */
double x[], /* Input array size n */
double fvec[], /* array of length n which must contain the functions evaluated at x[] */
double fjac[], /* output n by n array containing approximation to the Jacobian matrix at x[] */
int ldfjac, /* stride of fjac[] */
int ml, /* Number of subdiagonals within the band of the Jacobian matrix */
int mu, /* Number of superdiagonals within the band of the Jacobian matrix */
double epsfcn, /* Step length for the forward-difference approximation */
double *wa1, /* Working arrays of length n */
double *wa2)
{
/* Local variables */
int iflag = 0;
double temp;
int msum;
double h;
int i, j, k;
double eps;
int nm1 = n-1;
/* M_DIVER is the machine precision. */
eps = sqrt((max(epsfcn,M_DIVER)));
msum = ml + mu + 1;
if (msum >= n) {
/* Computation of dense approximate jacobian. */
for (j = 0; j < n; ++j) {
temp = x[j];
h = eps * fabs(temp);
if (h == 0.0)
h = eps;
x[j] = temp + h;
if ((iflag = (*fcn)(fdata,n, &x[0], &wa1[0], 1)) < 0)
break;
x[j] = temp;
for (i = 0; i < n; ++i)
fjac[i + j * ldfjac] = (wa1[i] - fvec[i]) / h;
}
} else {
/* Computation of banded approximate jacobian. */
for (k = 0; k < msum; ++k) {
for (j = k; msum < 0 ? j >= nm1 : j <= nm1; j += msum) {
wa2[j] = x[j];
h = eps * fabs(wa2[j]);
if (h == 0.0)
h = eps;
x[j] = wa2[j] + h;
}
if ((iflag = (*fcn)(fdata, n, &x[0], &wa1[0], 1)) < 0)
break;
for (j = k; msum < 0 ? j >= nm1 : j <= nm1; j += msum) {
x[j] = wa2[j];
h = eps * fabs(wa2[j]);
if (h == 0.0)
h = eps;
for (i = 0; i < n; ++i) {
fjac[i + j * ldfjac] = 0.0;
if (i >= j - mu && i <= j + ml)
fjac[i + j * ldfjac] = (wa1[i] - fvec[i]) / h;
}
}
}
}
return iflag;
} /* dfdjc1_ */
/***************************************************************/
/***************************************************************/
/*
* This subroutine proceeds from the computed QR factorization of
* an M by N matrix A to accumulate the M by M orthogonal matrix
* Q from its factored form.
*
*/
static int dqform(
int m, /* No of rows of A and the order of Q. */
int n, /* No of columns of A. */
double *q, /* m by m array */
int ldq, /* stride of q[][] */
double *wa) /* Working aray length m */
{
int i, j, k, l, minmn;
double sum;
/* Zero out upper triangle of q in the first min(m,n) columns. */
minmn = min(m,n);
if (minmn >= 2) {
for (j = 1; j < minmn; ++j) {
for (i = 0; i < j; ++i)
q[i + j * ldq] = 0.0;
}
}
/* Initialize remaining columns to those of the identity matrix. */
if (m > n) {
for (j = n; j < m; ++j) {
for (i = 0; i < m; ++i) {
q[i + j * ldq] = 0.0;
}
q[j + j * ldq] = 1.0;
}
}
/* Accumulate q from its factored form. */
for (l = 0; l < minmn; ++l) {
k = minmn - l - 1;
for (i = k; i < m; ++i) {
wa[i] = q[i + k * ldq];
q[i + k * ldq] = 0.0;
}
q[k + k * ldq] = 1.0;
if (wa[k] != 0.0) {
for (j = k; j < m; ++j) {
double temp;
sum = 0.0;
for (i = k; i < m; ++i)
sum += q[i + j * ldq] * wa[i];
temp = sum / wa[k];
for (i = k; i < m; ++i)
q[i + j * ldq] -= temp * wa[i];
}
}
}
return 0;
} /* dqform_ */
/***************************************************************/
/***************************************************************/
/*
* This subroutine uses Householder transformations with column
* pivoting (optional) to compute a QR factorization of the
* M by N matrix A. That is, DQRFAC determines an orthogonal
* matrix Q, a permutation matrix P, and an upper trapezoidal
* matrix R with diagonal elements of nonincreasing magnitude,
* such that A*P = Q*R. The Householder transformation for
* column K, K = 1,2,...,MIN(M,N), is of the form
*
* T
* I - (1/U(K))*U*U
*
* where U has zeros in the first K-1 positions. The form of
* this transformation and the method of pivoting first
* appeared in the corresponding LINPACK subroutine.
*
*/
static int dqrfac(
int m, /* Number of rows of a[] */
int n, /* Number of columns of a[] */
double *a, /* m by n array */
int lda, /* stride of a[][] */
abool pivot, /* TRUE to enforce column pivoting */
int *ipvt, /* Pivot output array, size n */
double *sigma, /* Output diagonal elements of R, length n */
double *acnorm, /* Output norms of A, length n */
double *wa) /* Working array size n */
{
/* Local variables */
int kmax;
int i, j, k, minmn;
double ajnorm;
int jp1;
double sum;
/* Compute the initial column norms and initialize several arrays. */
for (j = 0; j < n; ++j) {
acnorm[j] = denorm(m, &a[j * lda]);
sigma[j] = acnorm[j];
wa[j] = sigma[j];
if (pivot)
ipvt[j] = j;
}
/* Reduce a to r with householder transformations. */
minmn = min(m,n);
for (j = 0; j < minmn; ++j) {
if (pivot) {
/* Bring the column of largest norm into the pivot position. */
kmax = j;
for (k = j; k < n; ++k) {
if (sigma[k] > sigma[kmax]) {
kmax = k;
}
}
if (kmax != j) {
for (i = 0; i < m; ++i) {
double temp;
temp = a[i + j * lda];
a[i + j * lda] = a[i + kmax * lda];
a[i + kmax * lda] = temp;
}
sigma[kmax] = sigma[j];
wa[kmax] = wa[j];
k = ipvt[j];
ipvt[j] = ipvt[kmax];
ipvt[kmax] = k;
}
}
/* Compute the householder transformation to reduce the */
/* j-th column of a to a multiple of the j-th unit vector. */
ajnorm = denorm(m - j, &a[j + j * lda]);
if (ajnorm != 0.0) {
if (a[j + j * lda] < 0.0)
ajnorm = -ajnorm;
for (i = j; i < m; ++i)
a[i + j * lda] /= ajnorm;
a[j + j * lda] += 1.0;
/* Apply the transformation to the remaining columns */
/* and update the norms. */
jp1 = j + 1;
if (n > jp1) {
for (k = jp1; k < n; ++k) {
double temp;
sum = 0.0;
for (i = j; i < m; ++i)
sum += a[i + j * lda] * a[i + k * lda];
temp = sum / a[j + j * lda];
for (i = j; i < m; ++i)
a[i + k * lda] -= temp * a[i + j * lda];
if (pivot && sigma[k] != 0.0) {
temp = a[j + k * lda] / sigma[k];
temp = 1.0 - temp * temp;
sigma[k] *= sqrt((max(0.0,temp)));
temp = sigma[k] / wa[k];
if (0.05 * (temp * temp) <= M_DIVER) {
sigma[k] = denorm(m - jp1, &a[jp1 + k * lda]);
wa[k] = sigma[k];
}
}
}
}
}
sigma[j] = -ajnorm;
}
return 0;
} /* dqrfac_ */
/***************************************************************/
/***************************************************************/
Reference in New Issue View Git Blame Copy Permalink