mirror/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2024-12-19 03:58:31 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity
c7cf80fd59
opencv/3rdparty/lapack/dlasd2.c

597 lines
18 KiB
C
Raw Blame History

#include "clapack.h"
/* Table of constant values */
static integer c__1 = 1;
static doublereal c_b30 = 0.;
/* Subroutine */ int dlasd2_(integer *nl, integer *nr, integer *sqre, integer
*k, doublereal *d__, doublereal *z__, doublereal *alpha, doublereal *
beta, doublereal *u, integer *ldu, doublereal *vt, integer *ldvt,
doublereal *dsigma, doublereal *u2, integer *ldu2, doublereal *vt2,
integer *ldvt2, integer *idxp, integer *idx, integer *idxc, integer *
idxq, integer *coltyp, integer *info)
{
/* System generated locals */
integer u_dim1, u_offset, u2_dim1, u2_offset, vt_dim1, vt_offset,
vt2_dim1, vt2_offset, i__1;
doublereal d__1, d__2;
/* Local variables */
doublereal c__;
integer i__, j, m, n;
doublereal s;
integer k2;
doublereal z1;
integer ct, jp;
doublereal eps, tau, tol;
integer psm[4], nlp1, nlp2, idxi, idxj;
extern /* Subroutine */ int drot_(integer *, doublereal *, integer *,
doublereal *, integer *, doublereal *, doublereal *);
integer ctot[4], idxjp;
extern /* Subroutine */ int dcopy_(integer *, doublereal *, integer *,
doublereal *, integer *);
integer jprev;
extern doublereal dlapy2_(doublereal *, doublereal *), dlamch_(char *);
extern /* Subroutine */ int dlamrg_(integer *, integer *, doublereal *,
integer *, integer *, integer *), dlacpy_(char *, integer *,
integer *, doublereal *, integer *, doublereal *, integer *), dlaset_(char *, integer *, integer *, doublereal *,
doublereal *, doublereal *, integer *), xerbla_(char *,
integer *);
doublereal hlftol;
/* -- LAPACK auxiliary routine (version 3.1) -- */
/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
/* November 2006 */
/* .. Scalar Arguments .. */
/* .. */
/* .. Array Arguments .. */
/* .. */
/* Purpose */
/* ======= */
/* DLASD2 merges the two sets of singular values together into a single */
/* sorted set. Then it tries to deflate the size of the problem. */
/* There are two ways in which deflation can occur: when two or more */
/* singular values are close together or if there is a tiny entry in the */
/* Z vector. For each such occurrence the order of the related secular */
/* equation problem is reduced by one. */
/* DLASD2 is called from DLASD1. */
/* Arguments */
/* ========= */
/* NL (input) INTEGER */
/* The row dimension of the upper block. NL >= 1. */
/* NR (input) INTEGER */
/* The row dimension of the lower block. NR >= 1. */
/* SQRE (input) INTEGER */
/* = 0: the lower block is an NR-by-NR square matrix. */
/* = 1: the lower block is an NR-by-(NR+1) rectangular matrix. */
/* The bidiagonal matrix has N = NL + NR + 1 rows and */
/* M = N + SQRE >= N columns. */
/* K (output) INTEGER */
/* Contains the dimension of the non-deflated matrix, */
/* This is the order of the related secular equation. 1 <= K <=N. */
/* D (input/output) DOUBLE PRECISION array, dimension(N) */
/* On entry D contains the singular values of the two submatrices */
/* to be combined. On exit D contains the trailing (N-K) updated */
/* singular values (those which were deflated) sorted into */
/* increasing order. */
/* Z (output) DOUBLE PRECISION array, dimension(N) */
/* On exit Z contains the updating row vector in the secular */
/* equation. */
/* ALPHA (input) DOUBLE PRECISION */
/* Contains the diagonal element associated with the added row. */
/* BETA (input) DOUBLE PRECISION */
/* Contains the off-diagonal element associated with the added */
/* row. */
/* U (input/output) DOUBLE PRECISION array, dimension(LDU,N) */
/* On entry U contains the left singular vectors of two */
/* submatrices in the two square blocks with corners at (1,1), */
/* (NL, NL), and (NL+2, NL+2), (N,N). */
/* On exit U contains the trailing (N-K) updated left singular */
/* vectors (those which were deflated) in its last N-K columns. */
/* LDU (input) INTEGER */
/* The leading dimension of the array U. LDU >= N. */
/* VT (input/output) DOUBLE PRECISION array, dimension(LDVT,M) */
/* On entry VT' contains the right singular vectors of two */
/* submatrices in the two square blocks with corners at (1,1), */
/* (NL+1, NL+1), and (NL+2, NL+2), (M,M). */
/* On exit VT' contains the trailing (N-K) updated right singular */
/* vectors (those which were deflated) in its last N-K columns. */
/* In case SQRE =1, the last row of VT spans the right null */
/* space. */
/* LDVT (input) INTEGER */
/* The leading dimension of the array VT. LDVT >= M. */
/* DSIGMA (output) DOUBLE PRECISION array, dimension (N) */
/* Contains a copy of the diagonal elements (K-1 singular values */
/* and one zero) in the secular equation. */
/* U2 (output) DOUBLE PRECISION array, dimension(LDU2,N) */
/* Contains a copy of the first K-1 left singular vectors which */
/* will be used by DLASD3 in a matrix multiply (DGEMM) to solve */
/* for the new left singular vectors. U2 is arranged into four */
/* blocks. The first block contains a column with 1 at NL+1 and */
/* zero everywhere else; the second block contains non-zero */
/* entries only at and above NL; the third contains non-zero */
/* entries only below NL+1; and the fourth is dense. */
/* LDU2 (input) INTEGER */
/* The leading dimension of the array U2. LDU2 >= N. */
/* VT2 (output) DOUBLE PRECISION array, dimension(LDVT2,N) */
/* VT2' contains a copy of the first K right singular vectors */
/* which will be used by DLASD3 in a matrix multiply (DGEMM) to */
/* solve for the new right singular vectors. VT2 is arranged into */
/* three blocks. The first block contains a row that corresponds */
/* to the special 0 diagonal element in SIGMA; the second block */
/* contains non-zeros only at and before NL +1; the third block */
/* contains non-zeros only at and after NL +2. */
/* LDVT2 (input) INTEGER */
/* The leading dimension of the array VT2. LDVT2 >= M. */
/* IDXP (workspace) INTEGER array dimension(N) */
/* This will contain the permutation used to place deflated */
/* values of D at the end of the array. On output IDXP(2:K) */
/* points to the nondeflated D-values and IDXP(K+1:N) */
/* points to the deflated singular values. */
/* IDX (workspace) INTEGER array dimension(N) */
/* This will contain the permutation used to sort the contents of */
/* D into ascending order. */
/* IDXC (output) INTEGER array dimension(N) */
/* This will contain the permutation used to arrange the columns */
/* of the deflated U matrix into three groups: the first group */
/* contains non-zero entries only at and above NL, the second */
/* contains non-zero entries only below NL+2, and the third is */
/* dense. */
/* IDXQ (input/output) INTEGER array dimension(N) */
/* This contains the permutation which separately sorts the two */
/* sub-problems in D into ascending order. Note that entries in */
/* the first hlaf of this permutation must first be moved one */
/* position backward; and entries in the second half */
/* must first have NL+1 added to their values. */
/* COLTYP (workspace/output) INTEGER array dimension(N) */
/* As workspace, this will contain a label which will indicate */
/* which of the following types a column in the U2 matrix or a */
/* row in the VT2 matrix is: */
/* 1 : non-zero in the upper half only */
/* 2 : non-zero in the lower half only */
/* 3 : dense */
/* 4 : deflated */
/* On exit, it is an array of dimension 4, with COLTYP(I) being */
/* the dimension of the I-th type columns. */
/* INFO (output) INTEGER */
/* = 0: successful exit. */
/* < 0: if INFO = -i, the i-th argument had an illegal value. */
/* Further Details */
/* =============== */
/* Based on contributions by */
/* Ming Gu and Huan Ren, Computer Science Division, University of */
/* California at Berkeley, USA */
/* ===================================================================== */
/* .. Parameters .. */
/* .. */
/* .. Local Arrays .. */
/* .. */
/* .. Local Scalars .. */
/* .. */
/* .. External Functions .. */
/* .. */
/* .. External Subroutines .. */
/* .. */
/* .. Intrinsic Functions .. */
/* .. */
/* .. Executable Statements .. */
/* Test the input parameters. */
/* Parameter adjustments */
--d__;
--z__;
u_dim1 = *ldu;
u_offset = 1 + u_dim1;
u -= u_offset;
vt_dim1 = *ldvt;
vt_offset = 1 + vt_dim1;
vt -= vt_offset;
--dsigma;
u2_dim1 = *ldu2;
u2_offset = 1 + u2_dim1;
u2 -= u2_offset;
vt2_dim1 = *ldvt2;
vt2_offset = 1 + vt2_dim1;
vt2 -= vt2_offset;
--idxp;
--idx;
--idxc;
--idxq;
--coltyp;
/* Function Body */
*info = 0;
if (*nl < 1) {
*info = -1;
} else if (*nr < 1) {
*info = -2;
} else if (*sqre != 1 && *sqre != 0) {
*info = -3;
}
n = *nl + *nr + 1;
m = n + *sqre;
if (*ldu < n) {
*info = -10;
} else if (*ldvt < m) {
*info = -12;
} else if (*ldu2 < n) {
*info = -15;
} else if (*ldvt2 < m) {
*info = -17;
}
if (*info != 0) {
i__1 = -(*info);
xerbla_("DLASD2", &i__1);
return 0;
}
nlp1 = *nl + 1;
nlp2 = *nl + 2;
/* Generate the first part of the vector Z; and move the singular */
/* values in the first part of D one position backward. */
z1 = *alpha * vt[nlp1 + nlp1 * vt_dim1];
z__[1] = z1;
for (i__ = *nl; i__ >= 1; --i__) {
z__[i__ + 1] = *alpha * vt[i__ + nlp1 * vt_dim1];
d__[i__ + 1] = d__[i__];
idxq[i__ + 1] = idxq[i__] + 1;
/* L10: */
}
/* Generate the second part of the vector Z. */
i__1 = m;
for (i__ = nlp2; i__ <= i__1; ++i__) {
z__[i__] = *beta * vt[i__ + nlp2 * vt_dim1];
/* L20: */
}
/* Initialize some reference arrays. */
i__1 = nlp1;
for (i__ = 2; i__ <= i__1; ++i__) {
coltyp[i__] = 1;
/* L30: */
}
i__1 = n;
for (i__ = nlp2; i__ <= i__1; ++i__) {
coltyp[i__] = 2;
/* L40: */
}
/* Sort the singular values into increasing order */
i__1 = n;
for (i__ = nlp2; i__ <= i__1; ++i__) {
idxq[i__] += nlp1;
/* L50: */
}
/* DSIGMA, IDXC, IDXC, and the first column of U2 */
/* are used as storage space. */
i__1 = n;
for (i__ = 2; i__ <= i__1; ++i__) {
dsigma[i__] = d__[idxq[i__]];
u2[i__ + u2_dim1] = z__[idxq[i__]];
idxc[i__] = coltyp[idxq[i__]];
/* L60: */
}
dlamrg_(nl, nr, &dsigma[2], &c__1, &c__1, &idx[2]);
i__1 = n;
for (i__ = 2; i__ <= i__1; ++i__) {
idxi = idx[i__] + 1;
d__[i__] = dsigma[idxi];
z__[i__] = u2[idxi + u2_dim1];
coltyp[i__] = idxc[idxi];
/* L70: */
}
/* Calculate the allowable deflation tolerance */
eps = dlamch_("Epsilon");
/* Computing MAX */
d__1 = abs(*alpha), d__2 = abs(*beta);
tol = max(d__1,d__2);
/* Computing MAX */
d__2 = (d__1 = d__[n], abs(d__1));
tol = eps * 8. * max(d__2,tol);
/* There are 2 kinds of deflation -- first a value in the z-vector */
/* is small, second two (or more) singular values are very close */
/* together (their difference is small). */
/* If the value in the z-vector is small, we simply permute the */
/* array so that the corresponding singular value is moved to the */
/* end. */
/* If two values in the D-vector are close, we perform a two-sided */
/* rotation designed to make one of the corresponding z-vector */
/* entries zero, and then permute the array so that the deflated */
/* singular value is moved to the end. */
/* If there are multiple singular values then the problem deflates. */
/* Here the number of equal singular values are found. As each equal */
/* singular value is found, an elementary reflector is computed to */
/* rotate the corresponding singular subspace so that the */
/* corresponding components of Z are zero in this new basis. */
*k = 1;
k2 = n + 1;
i__1 = n;
for (j = 2; j <= i__1; ++j) {
if ((d__1 = z__[j], abs(d__1)) <= tol) {
/* Deflate due to small z component. */
--k2;
idxp[k2] = j;
coltyp[j] = 4;
if (j == n) {
goto L120;
}
} else {
jprev = j;
goto L90;
}
/* L80: */
}
L90:
j = jprev;
L100:
++j;
if (j > n) {
goto L110;
}
if ((d__1 = z__[j], abs(d__1)) <= tol) {
/* Deflate due to small z component. */
--k2;
idxp[k2] = j;
coltyp[j] = 4;
} else {
/* Check if singular values are close enough to allow deflation. */
if ((d__1 = d__[j] - d__[jprev], abs(d__1)) <= tol) {
/* Deflation is possible. */
s = z__[jprev];
c__ = z__[j];
/* Find sqrt(a**2+b**2) without overflow or */
/* destructive underflow. */
tau = dlapy2_(&c__, &s);
c__ /= tau;
s = -s / tau;
z__[j] = tau;
z__[jprev] = 0.;
/* Apply back the Givens rotation to the left and right */
/* singular vector matrices. */
idxjp = idxq[idx[jprev] + 1];
idxj = idxq[idx[j] + 1];
if (idxjp <= nlp1) {
--idxjp;
}
if (idxj <= nlp1) {
--idxj;
}
drot_(&n, &u[idxjp * u_dim1 + 1], &c__1, &u[idxj * u_dim1 + 1], &
c__1, &c__, &s);
drot_(&m, &vt[idxjp + vt_dim1], ldvt, &vt[idxj + vt_dim1], ldvt, &
c__, &s);
if (coltyp[j] != coltyp[jprev]) {
coltyp[j] = 3;
}
coltyp[jprev] = 4;
--k2;
idxp[k2] = jprev;
jprev = j;
} else {
++(*k);
u2[*k + u2_dim1] = z__[jprev];
dsigma[*k] = d__[jprev];
idxp[*k] = jprev;
jprev = j;
}
}
goto L100;
L110:
/* Record the last singular value. */
++(*k);
u2[*k + u2_dim1] = z__[jprev];
dsigma[*k] = d__[jprev];
idxp[*k] = jprev;
L120:
/* Count up the total number of the various types of columns, then */
/* form a permutation which positions the four column types into */
/* four groups of uniform structure (although one or more of these */
/* groups may be empty). */
for (j = 1; j <= 4; ++j) {
ctot[j - 1] = 0;
/* L130: */
}
i__1 = n;
for (j = 2; j <= i__1; ++j) {
ct = coltyp[j];
++ctot[ct - 1];
/* L140: */
}
/* PSM(*) = Position in SubMatrix (of types 1 through 4) */
psm[0] = 2;
psm[1] = ctot[0] + 2;
psm[2] = psm[1] + ctot[1];
psm[3] = psm[2] + ctot[2];
/* Fill out the IDXC array so that the permutation which it induces */
/* will place all type-1 columns first, all type-2 columns next, */
/* then all type-3's, and finally all type-4's, starting from the */
/* second column. This applies similarly to the rows of VT. */
i__1 = n;
for (j = 2; j <= i__1; ++j) {
jp = idxp[j];
ct = coltyp[jp];
idxc[psm[ct - 1]] = j;
++psm[ct - 1];
/* L150: */
}
/* Sort the singular values and corresponding singular vectors into */
/* DSIGMA, U2, and VT2 respectively. The singular values/vectors */
/* which were not deflated go into the first K slots of DSIGMA, U2, */
/* and VT2 respectively, while those which were deflated go into the */
/* last N - K slots, except that the first column/row will be treated */
/* separately. */
i__1 = n;
for (j = 2; j <= i__1; ++j) {
jp = idxp[j];
dsigma[j] = d__[jp];
idxj = idxq[idx[idxp[idxc[j]]] + 1];
if (idxj <= nlp1) {
--idxj;
}
dcopy_(&n, &u[idxj * u_dim1 + 1], &c__1, &u2[j * u2_dim1 + 1], &c__1);
dcopy_(&m, &vt[idxj + vt_dim1], ldvt, &vt2[j + vt2_dim1], ldvt2);
/* L160: */
}
/* Determine DSIGMA(1), DSIGMA(2) and Z(1) */
dsigma[1] = 0.;
hlftol = tol / 2.;
if (abs(dsigma[2]) <= hlftol) {
dsigma[2] = hlftol;
}
if (m > n) {
z__[1] = dlapy2_(&z1, &z__[m]);
if (z__[1] <= tol) {
c__ = 1.;
s = 0.;
z__[1] = tol;
} else {
c__ = z1 / z__[1];
s = z__[m] / z__[1];
}
} else {
if (abs(z1) <= tol) {
z__[1] = tol;
} else {
z__[1] = z1;
}
}
/* Move the rest of the updating row to Z. */
i__1 = *k - 1;
dcopy_(&i__1, &u2[u2_dim1 + 2], &c__1, &z__[2], &c__1);
/* Determine the first column of U2, the first row of VT2 and the */
/* last row of VT. */
dlaset_("A", &n, &c__1, &c_b30, &c_b30, &u2[u2_offset], ldu2);
u2[nlp1 + u2_dim1] = 1.;
if (m > n) {
i__1 = nlp1;
for (i__ = 1; i__ <= i__1; ++i__) {
vt[m + i__ * vt_dim1] = -s * vt[nlp1 + i__ * vt_dim1];
vt2[i__ * vt2_dim1 + 1] = c__ * vt[nlp1 + i__ * vt_dim1];
/* L170: */
}
i__1 = m;
for (i__ = nlp2; i__ <= i__1; ++i__) {
vt2[i__ * vt2_dim1 + 1] = s * vt[m + i__ * vt_dim1];
vt[m + i__ * vt_dim1] = c__ * vt[m + i__ * vt_dim1];
/* L180: */
}
} else {
dcopy_(&m, &vt[nlp1 + vt_dim1], ldvt, &vt2[vt2_dim1 + 1], ldvt2);
}
if (m > n) {
dcopy_(&m, &vt[m + vt_dim1], ldvt, &vt2[m + vt2_dim1], ldvt2);
}
/* The deflated singular values and their corresponding vectors go */
/* into the back of D, U, and V respectively. */
if (n > *k) {
i__1 = n - *k;
dcopy_(&i__1, &dsigma[*k + 1], &c__1, &d__[*k + 1], &c__1);
i__1 = n - *k;
dlacpy_("A", &n, &i__1, &u2[(*k + 1) * u2_dim1 + 1], ldu2, &u[(*k + 1)
* u_dim1 + 1], ldu);
i__1 = n - *k;
dlacpy_("A", &i__1, &m, &vt2[*k + 1 + vt2_dim1], ldvt2, &vt[*k + 1 +
vt_dim1], ldvt);
}
/* Copy CTOT into COLTYP for referencing in DLASD3. */
for (j = 1; j <= 4; ++j) {
coltyp[j] = ctot[j - 1];
/* L190: */
}
return 0;
/* End of DLASD2 */
} /* dlasd2_ */
Reference in New Issue View Git Blame Copy Permalink