Eigen::VectorXd DistributedDiff::diff(const unsigned long long &u_ts, const Eigen::VectorXd &samples) {
addDataToBuffer(u_ts, samples);
// Next we want to calculate all the differentials
Eigen::VectorXd diff_(size);
diff_ = FindDifferentials();
return diff_;
}
/* ------------------------------------------------------------------ */
/* Subroutine */ int nnls_(doublereal *a, const integer *mda, const integer *m, const integer *n, doublereal* b, doublereal* x, doublereal* rnorm, doublereal* w, doublereal* zz, integer* index, integer* mode)
{
/* System generated locals */
integer a_dim1, a_offset, i__1, i__2;
doublereal d__1, d__2;
/* Builtin functions */
/* The following lines were commented out after the f2c translation */
/* double sqrt(); */
/* integer s_wsfe(), do_fio(), e_wsfe(); */
/* Local variables */
//extern doublereal diff_();
/*static*/ integer iter;
/*static*/ doublereal temp, wmax;
/*static*/ integer i__, j, l;
/*static*/ doublereal t, alpha, asave;
/*static*/ integer itmax, izmax, nsetp;
//extern /* Subroutine */ int g1_();
/*static*/ doublereal dummy, unorm, ztest, cc;
//extern /* Subroutine */ int h12_();
/*static*/ integer ii, jj, ip;
/*static*/ doublereal sm;
/*static*/ integer iz, jz;
/*static*/ doublereal up, ss;
/*static*/ integer rtnkey, iz1, iz2, npp1;
/* Fortran I/O blocks */
/* The following line was commented out after the f2c translation */
/* static cilist io___22 = { 0, 6, 0, "(/a)", 0 }; */
/* ------------------------------------------------------------------
*/
/* integer INDEX(N) */
/* double precision A(MDA,N), B(M), W(N), X(N), ZZ(M) */
/* ------------------------------------------------------------------
*/
/* Parameter adjustments */
a_dim1 = *mda;
a_offset = a_dim1 + 1;
a -= a_offset;
--b;
--x;
--w;
--zz;
--index;
/* Function Body */
*mode = 1;
if (*m <= 0 || *n <= 0) {
*mode = 2;
return 0;
}
iter = 0;
itmax = *n * 1; //3
/* INITIALIZE THE ARRAYS INDEX() AND X(). */
i__1 = *n;
for (i__ = 1; i__ <= i__1; ++i__) {
x[i__] = 0.;
/* L20: */
index[i__] = i__;
}
iz2 = *n;
iz1 = 1;
nsetp = 0;
npp1 = 1;
/* ****** MAIN LOOP BEGINS HERE ****** */
L30:
/* QUIT IF ALL COEFFICIENTS ARE ALREADY IN THE SOLUTION.
*/
/* OR IF M COLS OF A HAVE BEEN TRIANGULARIZED. */
if (iz1 > iz2 || nsetp >= *m) {
goto L350;
}
/* COMPUTE COMPONENTS OF THE DUAL (NEGATIVE GRADIENT) VECTOR W().
*/
i__1 = iz2;
for (iz = iz1; iz <= i__1; ++iz) {
j = index[iz];
sm = 0.;
i__2 = *m;
for (l = npp1; l <= i__2; ++l) {
/* L40: */
sm += a[l + j * a_dim1] * b[l];
}
w[j] = sm;
/* L50: */
}
/* FIND LARGEST POSITIVE W(J). */
L60:
wmax = 0.;
i__1 = iz2;
for (iz = iz1; iz <= i__1; ++iz) {
j = index[iz];
if (w[j] > wmax) {
wmax = w[j];
izmax = iz;
}
/* L70: */
}
/* IF WMAX .LE. 0. GO TO TERMINATION. */
/* THIS INDICATES SATISFACTION OF THE KUHN-TUCKER CONDITIONS.
*/
if (wmax <= 0.) {
goto L350;
}
iz = izmax;
j = index[iz];
/* THE SIGN OF W(J) IS OK FOR J TO BE MOVED TO SET P. */
/* BEGIN THE TRANSFORMATION AND CHECK NEW DIAGONAL ELEMENT TO AVOID */
/* NEAR LINEAR DEPENDENCE. */
asave = a[npp1 + j * a_dim1];
i__1 = npp1 + 1;
h12_(&c__1, &npp1, &i__1, m, &a[j * a_dim1 + 1], &c__1, &up, &dummy, &
c__1, &c__1, &c__0);
unorm = 0.;
if (nsetp != 0) {
i__1 = nsetp;
for (l = 1; l <= i__1; ++l) {
/* L90: */
/* Computing 2nd power */
d__1 = a[l + j * a_dim1];
unorm += d__1 * d__1;
}
}
unorm = sqrt(unorm);
d__2 = unorm + (d__1 = a[npp1 + j * a_dim1], nnls_abs(d__1)) * .01;
if (diff_(&d__2, &unorm) > 0.) {
/* COL J IS SUFFICIENTLY INDEPENDENT. COPY B INTO ZZ, UPDATE Z
Z */
/* AND SOLVE FOR ZTEST ( = PROPOSED NEW VALUE FOR X(J) ). */
i__1 = *m;
for (l = 1; l <= i__1; ++l) {
/* L120: */
zz[l] = b[l];
}
i__1 = npp1 + 1;
h12_(&c__2, &npp1, &i__1, m, &a[j * a_dim1 + 1], &c__1, &up, &zz[1], &
c__1, &c__1, &c__1);
ztest = zz[npp1] / a[npp1 + j * a_dim1];
/* SEE IF ZTEST IS POSITIVE */
if (ztest > 0.) {
goto L140;
}
}
/* REJECT J AS A CANDIDATE TO BE MOVED FROM SET Z TO SET P. */
/* RESTORE A(NPP1,J), SET W(J)=0., AND LOOP BACK TO TEST DUAL */
/* COEFFS AGAIN. */
a[npp1 + j * a_dim1] = asave;
w[j] = 0.;
goto L60;
/* THE INDEX J=INDEX(IZ) HAS BEEN SELECTED TO BE MOVED FROM */
/* SET Z TO SET P. UPDATE B, UPDATE INDICES, APPLY HOUSEHOLDER */
/* TRANSFORMATIONS TO COLS IN NEW SET Z, ZERO SUBDIAGONAL ELTS IN */
/* COL J, SET W(J)=0. */
L140:
i__1 = *m;
for (l = 1; l <= i__1; ++l) {
/* L150: */
b[l] = zz[l];
}
index[iz] = index[iz1];
index[iz1] = j;
++iz1;
nsetp = npp1;
++npp1;
if (iz1 <= iz2) {
i__1 = iz2;
for (jz = iz1; jz <= i__1; ++jz) {
jj = index[jz];
h12_(&c__2, &nsetp, &npp1, m, &a[j * a_dim1 + 1], &c__1, &up, &a[
jj * a_dim1 + 1], &c__1, mda, &c__1);
/* L160: */
}
}
if (nsetp != *m) {
i__1 = *m;
for (l = npp1; l <= i__1; ++l) {
/* L180: */
a[l + j * a_dim1] = 0.;
}
}
w[j] = 0.;
/* SOLVE THE TRIANGULAR SYSTEM. */
/* STORE THE SOLUTION TEMPORARILY IN ZZ().
*/
rtnkey = 1;
goto L400;
L200:
/* ****** SECONDARY LOOP BEGINS HERE ****** */
/* ITERATION COUNTER. */
L210:
++iter;
if (iter > itmax) {
*mode = 3;
/* The following lines were replaced after the f2c translation */
/* s_wsfe(&io___22); */
/* do_fio(&c__1, " NNLS quitting on iteration count.", 34L); */
/* e_wsfe(); */
fprintf(stdout, "\n NNLS quitting on iteration count.\n");
fflush(stdout);
goto L350;
}
/* SEE IF ALL NEW CONSTRAINED COEFFS ARE FEASIBLE. */
/* IF NOT COMPUTE ALPHA. */
alpha = 2.;
i__1 = nsetp;
for (ip = 1; ip <= i__1; ++ip) {
l = index[ip];
if (zz[ip] <= 0.) {
t = -x[l] / (zz[ip] - x[l]);
if (alpha > t) {
alpha = t;
jj = ip;
}
}
/* L240: */
}
/* IF ALL NEW CONSTRAINED COEFFS ARE FEASIBLE THEN ALPHA WILL */
/* STILL = 2. IF SO EXIT FROM SECONDARY LOOP TO MAIN LOOP. */
if (alpha == 2.) {
goto L330;
}
/* OTHERWISE USE ALPHA WHICH WILL BE BETWEEN 0. AND 1. TO */
/* INTERPOLATE BETWEEN THE OLD X AND THE NEW ZZ. */
i__1 = nsetp;
for (ip = 1; ip <= i__1; ++ip) {
l = index[ip];
x[l] += alpha * (zz[ip] - x[l]);
/* L250: */
}
/* MODIFY A AND B AND THE INDEX ARRAYS TO MOVE COEFFICIENT I */
/* FROM SET P TO SET Z. */
i__ = index[jj];
L260:
x[i__] = 0.;
if (jj != nsetp) {
++jj;
i__1 = nsetp;
for (j = jj; j <= i__1; ++j) {
ii = index[j];
index[j - 1] = ii;
g1_(&a[j - 1 + ii * a_dim1], &a[j + ii * a_dim1], &cc, &ss, &a[j
- 1 + ii * a_dim1]);
a[j + ii * a_dim1] = 0.;
i__2 = *n;
for (l = 1; l <= i__2; ++l) {
if (l != ii) {
/* Apply procedure G2 (CC,SS,A(J-1,L),A(J,
L)) */
temp = a[j - 1 + l * a_dim1];
a[j - 1 + l * a_dim1] = cc * temp + ss * a[j + l * a_dim1]
;
a[j + l * a_dim1] = -ss * temp + cc * a[j + l * a_dim1];
}
/* L270: */
}
/* Apply procedure G2 (CC,SS,B(J-1),B(J)) */
temp = b[j - 1];
b[j - 1] = cc * temp + ss * b[j];
b[j] = -ss * temp + cc * b[j];
/* L280: */
}
}
npp1 = nsetp;
--nsetp;
--iz1;
index[iz1] = i__;
/* SEE IF THE REMAINING COEFFS IN SET P ARE FEASIBLE. THEY SHOULD
*/
/* BE BECAUSE OF THE WAY ALPHA WAS DETERMINED. */
/* IF ANY ARE INFEASIBLE IT IS DUE TO ROUND-OFF ERROR. ANY */
/* THAT ARE NONPOSITIVE WILL BE SET TO ZERO */
/* AND MOVED FROM SET P TO SET Z. */
i__1 = nsetp;
for (jj = 1; jj <= i__1; ++jj) {
i__ = index[jj];
if (x[i__] <= 0.) {
goto L260;
}
/* L300: */
}
/* COPY B( ) INTO ZZ( ). THEN SOLVE AGAIN AND LOOP BACK. */
i__1 = *m;
for (i__ = 1; i__ <= i__1; ++i__) {
/* L310: */
zz[i__] = b[i__];
}
rtnkey = 2;
goto L400;
L320:
goto L210;
/* ****** END OF SECONDARY LOOP ****** */
L330:
i__1 = nsetp;
for (ip = 1; ip <= i__1; ++ip) {
i__ = index[ip];
/* L340: */
x[i__] = zz[ip];
}
/* ALL NEW COEFFS ARE POSITIVE. LOOP BACK TO BEGINNING. */
goto L30;
/* ****** END OF MAIN LOOP ****** */
/* COME TO HERE FOR TERMINATION. */
/* COMPUTE THE NORM OF THE FINAL RESIDUAL VECTOR. */
L350:
sm = 0.;
if (npp1 <= *m) {
i__1 = *m;
for (i__ = npp1; i__ <= i__1; ++i__) {
/* L360: */
/* Computing 2nd power */
d__1 = b[i__];
sm += d__1 * d__1;
}
} else {
i__1 = *n;
for (j = 1; j <= i__1; ++j) {
/* L380: */
w[j] = 0.;
}
}
*rnorm = sqrt(sm);
return 0;
/* THE FOLLOWING BLOCK OF CODE IS USED AS AN INTERNAL SUBROUTINE */
/* TO SOLVE THE TRIANGULAR SYSTEM, PUTTING THE SOLUTION IN ZZ(). */
L400:
i__1 = nsetp;
for (l = 1; l <= i__1; ++l) {
ip = nsetp + 1 - l;
if (l != 1) {
i__2 = ip;
for (ii = 1; ii <= i__2; ++ii) {
zz[ii] -= a[ii + jj * a_dim1] * zz[ip + 1];
/* L410: */
}
}
jj = index[ip];
zz[ip] /= a[ip + jj * a_dim1];
/* L430: */
}
switch ((int)rtnkey) {
case 1: goto L200;
case 2: goto L320;
}
/* The next line was added after the f2c translation to keep
compilers from complaining about a void return from a non-void
function. */
return 0;
} /* nnls_ */
float dgFastRayTest::PolygonIntersect (const dgVector& normal, const float* const polygon, int32_t strideInBytes, const int32_t* const indexArray, int32_t indexCount) const
{
HACD_ASSERT (m_p0.m_w == m_p1.m_w);
#ifndef __USE_DOUBLE_PRECISION__
float unrealible = float (1.0e10f);
#endif
float dist = normal % m_diff;
if (dist < m_dirError) {
int32_t stride = int32_t (strideInBytes / sizeof (float));
dgVector v0 (&polygon[indexArray[indexCount - 1] * stride]);
dgVector p0v0 (v0 - m_p0);
float tOut = normal % p0v0;
// this only work for convex polygons and for single side faces
// walk the polygon around the edges and calculate the volume
if ((tOut < float (0.0f)) && (tOut > dist)) {
for (int32_t i = 0; i < indexCount; i ++) {
int32_t i2 = indexArray[i] * stride;
dgVector v1 (&polygon[i2]);
dgVector p0v1 (v1 - m_p0);
// calculate the volume formed by the line and the edge of the polygon
float alpha = (m_diff * p0v1) % p0v0;
// if a least one volume is negative it mean the line cross the polygon outside this edge and do not hit the face
if (alpha < DG_RAY_TOL_ERROR) {
#ifdef __USE_DOUBLE_PRECISION__
return 1.2f;
#else
unrealible = alpha;
break;
#endif
}
p0v0 = p0v1;
}
#ifndef __USE_DOUBLE_PRECISION__
if ((unrealible < float (0.0f)) && (unrealible > (DG_RAY_TOL_ERROR * float (10.0f)))) {
// the edge is too close to an edge float is not reliable, do the calculation with double
dgBigVector v0_ (v0);
dgBigVector m_p0_ (m_p0);
//dgBigVector m_p1_ (m_p1);
dgBigVector p0v0_ (v0_ - m_p0_);
dgBigVector normal_ (normal);
dgBigVector diff_ (m_diff);
double tOut_ = normal_ % p0v0_;
//double dist_ = normal_ % diff_;
if ((tOut < double (0.0f)) && (tOut > dist)) {
for (int32_t i = 0; i < indexCount; i ++) {
int32_t i2 = indexArray[i] * stride;
dgBigVector v1 (&polygon[i2]);
dgBigVector p0v1_ (v1 - m_p0_);
// calculate the volume formed by the line and the edge of the polygon
double alpha = (diff_ * p0v1_) % p0v0_;
// if a least one volume is negative it mean the line cross the polygon outside this edge and do not hit the face
if (alpha < DG_RAY_TOL_ERROR) {
return 1.2f;
}
p0v0_ = p0v1_;
}
tOut = float (tOut_);
}
}
#endif
//the line is to the left of all the polygon edges,
//then the intersection is the point we the line intersect the plane of the polygon
tOut = tOut / dist;
HACD_ASSERT (tOut >= float (0.0f));
HACD_ASSERT (tOut <= float (1.0f));
return tOut;
}
}
return float (1.2f);
}
/*< SUBROUTINE LDP (G,MDG,M,N,H,X,XNORM,W,INDEX,MODE) >*/
/* Subroutine */ int ldp_(doublereal *g, integer *mdg, integer *m, integer *n,
doublereal *h__, doublereal *x, doublereal *xnorm, doublereal *w,
integer *index, integer *mode)
{
/* System generated locals */
integer g_dim1, g_offset, i__1, i__2;
doublereal d__1;
/* Builtin functions */
double sqrt(doublereal);
/* Local variables */
integer i__, j, jf, iw, iy, iz, np1;
doublereal fac;
extern doublereal diff_(doublereal *, doublereal *);
extern /* Subroutine */ int nnls_(doublereal *, integer *, integer *,
integer *, doublereal *, doublereal *, doublereal *, doublereal *,
doublereal *, integer *, integer *);
doublereal rnorm;
integer iwdual;
/* Algorithm LDP: LEAST DISTANCE PROGRAMMING */
/* The original version of this code was developed by */
/* Charles L. Lawson and Richard J. Hanson at Jet Propulsion Laboratory */
/* 1974 MAR 1, and published in the book */
/* "SOLVING LEAST SQUARES PROBLEMS", Prentice-HalL, 1974. */
/* Revised FEB 1995 to accompany reprinting of the book by SIAM. */
/* ------------------------------------------------------------------ */
/*< integer I, IW, IWDUAL, IY, IZ, J, JF, M, MDG, MODE, N, NP1 >*/
/* integer INDEX(M) */
/* double precision G(MDG,N), H(M), X(N), W(*) */
/*< integer INDEX(*) >*/
/*< double precision G(MDG,*), H(*), X(*), W(*) >*/
/*< double precision DIFF, FAC, ONE, RNORM, XNORM, ZERO >*/
/*< parameter(ONE = 1.0d0, ZERO = 0.0d0) >*/
/* ------------------------------------------------------------------ */
/*< IF (N.LE.0) GO TO 120 >*/
#line 19 "ldp.f"
/* Parameter adjustments */
#line 19 "ldp.f"
g_dim1 = *mdg;
#line 19 "ldp.f"
g_offset = 1 + g_dim1;
#line 19 "ldp.f"
g -= g_offset;
#line 19 "ldp.f"
--h__;
#line 19 "ldp.f"
--x;
#line 19 "ldp.f"
--w;
#line 19 "ldp.f"
--index;
#line 19 "ldp.f"
#line 19 "ldp.f"
/* Function Body */
#line 19 "ldp.f"
if (*n <= 0) {
#line 19 "ldp.f"
goto L120;
#line 19 "ldp.f"
}
/*< DO 10 J=1,N >*/
#line 20 "ldp.f"
i__1 = *n;
#line 20 "ldp.f"
for (j = 1; j <= i__1; ++j) {
/*< 10 X(J)=ZERO >*/
#line 21 "ldp.f"
/* L10: */
#line 21 "ldp.f"
x[j] = 0.;
#line 21 "ldp.f"
}
/*< XNORM=ZERO >*/
#line 22 "ldp.f"
*xnorm = 0.;
/*< IF (M.LE.0) GO TO 110 >*/
#line 23 "ldp.f"
if (*m <= 0) {
#line 23 "ldp.f"
goto L110;
#line 23 "ldp.f"
}
/* THE DECLARED DIMENSION OF W() MUST BE AT LEAST (N+1)*(M+2)+2*M. */
/* FIRST (N+1)*M LOCS OF W() = MATRIX E FOR PROBLEM NNLS. */
/* NEXT N+1 LOCS OF W() = VECTOR F FOR PROBLEM NNLS. */
/* NEXT N+1 LOCS OF W() = VECTOR Z FOR PROBLEM NNLS. */
/* NEXT M LOCS OF W() = VECTOR Y FOR PROBLEM NNLS. */
/* NEXT M LOCS OF W() = VECTOR WDUAL FOR PROBLEM NNLS. */
/* COPY G**T INTO FIRST N ROWS AND M COLUMNS OF E. */
/* COPY H**T INTO ROW N+1 OF E. */
/*< IW=0 >*/
#line 35 "ldp.f"
iw = 0;
/*< DO 30 J=1,M >*/
#line 36 "ldp.f"
i__1 = *m;
#line 36 "ldp.f"
for (j = 1; j <= i__1; ++j) {
/*< DO 20 I=1,N >*/
#line 37 "ldp.f"
i__2 = *n;
#line 37 "ldp.f"
for (i__ = 1; i__ <= i__2; ++i__) {
/*< IW=IW+1 >*/
#line 38 "ldp.f"
++iw;
/*< 20 W(IW)=G(J,I) >*/
#line 39 "ldp.f"
/* L20: */
#line 39 "ldp.f"
w[iw] = g[j + i__ * g_dim1];
#line 39 "ldp.f"
}
/*< IW=IW+1 >*/
#line 40 "ldp.f"
++iw;
/*< 30 W(IW)=H(J) >*/
#line 41 "ldp.f"
/* L30: */
#line 41 "ldp.f"
w[iw] = h__[j];
#line 41 "ldp.f"
}
/*< JF=IW+1 >*/
#line 42 "ldp.f"
jf = iw + 1;
/* STORE N ZEROS FOLLOWED BY A ONE INTO F. */
/*< DO 40 I=1,N >*/
#line 44 "ldp.f"
i__1 = *n;
#line 44 "ldp.f"
for (i__ = 1; i__ <= i__1; ++i__) {
/*< IW=IW+1 >*/
#line 45 "ldp.f"
++iw;
/*< 40 W(IW)=ZERO >*/
#line 46 "ldp.f"
/* L40: */
#line 46 "ldp.f"
w[iw] = 0.;
#line 46 "ldp.f"
}
/*< W(IW+1)=ONE >*/
#line 47 "ldp.f"
w[iw + 1] = 1.;
/*< NP1=N+1 >*/
#line 49 "ldp.f"
np1 = *n + 1;
/*< IZ=IW+2 >*/
#line 50 "ldp.f"
iz = iw + 2;
/*< IY=IZ+NP1 >*/
#line 51 "ldp.f"
iy = iz + np1;
/*< IWDUAL=IY+M >*/
#line 52 "ldp.f"
iwdual = iy + *m;
/*< >*/
#line 54 "ldp.f"
nnls_(&w[1], &np1, &np1, m, &w[jf], &w[iy], &rnorm, &w[iwdual], &w[iz], &
index[1], mode);
/* USE THE FOLLOWING RETURN IF UNSUCCESSFUL IN NNLS. */
/*< IF (MODE.NE.1) RETURN >*/
#line 57 "ldp.f"
if (*mode != 1) {
#line 57 "ldp.f"
return 0;
#line 57 "ldp.f"
}
/*< IF (RNORM) 130,130,50 >*/
#line 58 "ldp.f"
if (rnorm <= 0.) {
#line 58 "ldp.f"
goto L130;
#line 58 "ldp.f"
} else {
#line 58 "ldp.f"
goto L50;
#line 58 "ldp.f"
}
/*< 50 FAC=ONE >*/
#line 59 "ldp.f"
L50:
#line 59 "ldp.f"
fac = 1.;
/*< IW=IY-1 >*/
#line 60 "ldp.f"
iw = iy - 1;
/*< DO 60 I=1,M >*/
#line 61 "ldp.f"
i__1 = *m;
#line 61 "ldp.f"
for (i__ = 1; i__ <= i__1; ++i__) {
/*< IW=IW+1 >*/
#line 62 "ldp.f"
++iw;
/* HERE WE ARE USING THE SOLUTION VECTOR Y. */
/*< 60 FAC=FAC-H(I)*W(IW) >*/
#line 64 "ldp.f"
/* L60: */
#line 64 "ldp.f"
fac -= h__[i__] * w[iw];
#line 64 "ldp.f"
}
/*< IF (DIFF(ONE+FAC,ONE)) 130,130,70 >*/
#line 66 "ldp.f"
d__1 = fac + 1.;
#line 66 "ldp.f"
if (diff_(&d__1, &c_b12) <= 0.) {
#line 66 "ldp.f"
goto L130;
#line 66 "ldp.f"
} else {
#line 66 "ldp.f"
goto L70;
#line 66 "ldp.f"
}
/*< 70 FAC=ONE/FAC >*/
#line 67 "ldp.f"
L70:
#line 67 "ldp.f"
fac = 1. / fac;
/*< DO 90 J=1,N >*/
#line 68 "ldp.f"
i__1 = *n;
#line 68 "ldp.f"
for (j = 1; j <= i__1; ++j) {
/*< IW=IY-1 >*/
#line 69 "ldp.f"
iw = iy - 1;
/*< DO 80 I=1,M >*/
#line 70 "ldp.f"
i__2 = *m;
#line 70 "ldp.f"
for (i__ = 1; i__ <= i__2; ++i__) {
/*< IW=IW+1 >*/
#line 71 "ldp.f"
++iw;
/* HERE WE ARE USING THE SOLUTION VECTOR Y. */
/*< 80 X(J)=X(J)+G(I,J)*W(IW) >*/
#line 73 "ldp.f"
/* L80: */
#line 73 "ldp.f"
x[j] += g[i__ + j * g_dim1] * w[iw];
#line 73 "ldp.f"
}
/*< 90 X(J)=X(J)*FAC >*/
#line 74 "ldp.f"
/* L90: */
#line 74 "ldp.f"
x[j] *= fac;
#line 74 "ldp.f"
}
/*< DO 100 J=1,N >*/
#line 75 "ldp.f"
i__1 = *n;
#line 75 "ldp.f"
for (j = 1; j <= i__1; ++j) {
/*< 100 XNORM=XNORM+X(J)**2 >*/
#line 76 "ldp.f"
/* L100: */
/* Computing 2nd power */
#line 76 "ldp.f"
d__1 = x[j];
#line 76 "ldp.f"
*xnorm += d__1 * d__1;
#line 76 "ldp.f"
}
/*< XNORM=sqrt(XNORM) >*/
#line 77 "ldp.f"
*xnorm = sqrt(*xnorm);
/* SUCCESSFUL RETURN. */
/*< 110 MODE=1 >*/
#line 79 "ldp.f"
L110:
#line 79 "ldp.f"
*mode = 1;
/*< RETURN >*/
#line 80 "ldp.f"
return 0;
/* ERROR RETURN. N .LE. 0. */
/*< 120 MODE=2 >*/
#line 82 "ldp.f"
L120:
#line 82 "ldp.f"
*mode = 2;
/*< RETURN >*/
#line 83 "ldp.f"
return 0;
/* RETURNING WITH CONSTRAINTS NOT COMPATIBLE. */
/*< 130 MODE=4 >*/
#line 85 "ldp.f"
L130:
#line 85 "ldp.f"
*mode = 4;
/*< RETURN >*/
#line 86 "ldp.f"
return 0;
/*< END >*/
} /* ldp_ */
/* ------------------------------------------------------------------ */
/* Subroutine */ int qrbd_(integer *ipass, doublereal *q, doublereal *e,
integer *nn, doublereal *v, integer *mdv, integer *nrv, doublereal *
c__, integer *mdc, integer *ncc)
{
/* System generated locals */
integer c_dim1, c_offset, v_dim1, v_offset, i__1, i__2, i__3;
doublereal d__1, d__2, d__3;
/* Builtin functions */
double sqrt(doublereal);
/* Local variables */
static doublereal f, g, h__;
static integer i__, j, k, l, n;
static doublereal t, x, y, z__;
extern /* Subroutine */ int g1_(doublereal *, doublereal *, doublereal *,
doublereal *, doublereal *);
static integer n10, ii, kk;
static doublereal cs;
static integer ll;
static doublereal sn;
static integer lp1;
extern doublereal diff_(doublereal *, doublereal *);
static logical fail;
static doublereal temp;
static integer nqrs;
static logical wntv;
static doublereal small, dnorm;
static logical havers;
/* ------------------------------------------------------------------ */
/* double precision C(MDC,NCC), E(NN), Q(NN),V(MDV,NN) */
/* ------------------------------------------------------------------ */
/* Parameter adjustments */
--q;
--e;
v_dim1 = *mdv;
v_offset = 1 + v_dim1;
v -= v_offset;
c_dim1 = *mdc;
c_offset = 1 + c_dim1;
c__ -= c_offset;
/* Function Body */
n = *nn;
*ipass = 1;
if (n <= 0) {
return 0;
}
n10 = n * 10;
wntv = *nrv > 0;
havers = *ncc > 0;
fail = FALSE_;
nqrs = 0;
e[1] = 0.;
dnorm = 0.;
i__1 = n;
for (j = 1; j <= i__1; ++j) {
/* L10: */
/* Computing MAX */
d__3 = (d__1 = q[j], abs(d__1)) + (d__2 = e[j], abs(d__2));
dnorm = max(d__3,dnorm);
}
i__1 = n;
for (kk = 1; kk <= i__1; ++kk) {
k = n + 1 - kk;
/* TEST FOR SPLITTING OR RANK DEFICIENCIES.. */
/* FIRST MAKE TEST FOR LAST DIAGONAL TERM, Q(K), BEING SMALL. */
L20:
if (k == 1) {
goto L50;
}
d__1 = dnorm + q[k];
if (diff_(&d__1, &dnorm) != 0.) {
goto L50;
}
/* SINCE Q(K) IS SMALL WE WILL MAKE A SPECIAL PASS TO */
/* TRANSFORM E(K) TO ZERO. */
cs = 0.;
sn = -1.;
i__2 = k;
for (ii = 2; ii <= i__2; ++ii) {
i__ = k + 1 - ii;
f = -sn * e[i__ + 1];
e[i__ + 1] = cs * e[i__ + 1];
g1_(&q[i__], &f, &cs, &sn, &q[i__]);
/* TRANSFORMATION CONSTRUCTED TO ZERO POSITION (I,K). */
if (! wntv) {
goto L40;
}
i__3 = *nrv;
for (j = 1; j <= i__3; ++j) {
/* Apply procedure G2 (CS,SN,V(J,I),V(J,K)) */
temp = v[j + i__ * v_dim1];
v[j + i__ * v_dim1] = cs * temp + sn * v[j + k * v_dim1];
v[j + k * v_dim1] = -sn * temp + cs * v[j + k * v_dim1];
/* L30: */
}
/* ACCUMULATE RT. TRANSFORMATIONS IN V. */
L40:
;
}
/* THE MATRIX IS NOW BIDIAGONAL, AND OF LOWER ORDER */
/* SINCE E(K) .EQ. ZERO.. */
L50:
i__2 = k;
for (ll = 1; ll <= i__2; ++ll) {
l = k + 1 - ll;
d__1 = dnorm + e[l];
if (diff_(&d__1, &dnorm) == 0.) {
goto L100;
}
d__1 = dnorm + q[l - 1];
if (diff_(&d__1, &dnorm) == 0.) {
goto L70;
}
/* L60: */
}
/* THIS LOOP CAN'T COMPLETE SINCE E(1) = ZERO. */
goto L100;
/* CANCELLATION OF E(L), L.GT.1. */
L70:
cs = 0.;
sn = -1.;
i__2 = k;
for (i__ = l; i__ <= i__2; ++i__) {
f = -sn * e[i__];
e[i__] = cs * e[i__];
d__1 = dnorm + f;
if (diff_(&d__1, &dnorm) == 0.) {
goto L100;
}
g1_(&q[i__], &f, &cs, &sn, &q[i__]);
if (havers) {
i__3 = *ncc;
for (j = 1; j <= i__3; ++j) {
/* Apply procedure G2 ( CS, SN, C(I,J), C(L-1,J) */
temp = c__[i__ + j * c_dim1];
c__[i__ + j * c_dim1] = cs * temp + sn * c__[l - 1 + j *
c_dim1];
c__[l - 1 + j * c_dim1] = -sn * temp + cs * c__[l - 1 + j
* c_dim1];
/* L80: */
}
}
/* L90: */
}
/* TEST FOR CONVERGENCE.. */
L100:
z__ = q[k];
if (l == k) {
goto L170;
}
/* SHIFT FROM BOTTOM 2 BY 2 MINOR OF B**(T)*B. */
x = q[l];
y = q[k - 1];
g = e[k - 1];
h__ = e[k];
f = ((y - z__) * (y + z__) + (g - h__) * (g + h__)) / (h__ * 2. * y);
/* Computing 2nd power */
d__1 = f;
g = sqrt(d__1 * d__1 + 1.);
if (f >= 0.) {
t = f + g;
} else {
t = f - g;
}
f = ((x - z__) * (x + z__) + h__ * (y / t - h__)) / x;
/* NEXT QR SWEEP.. */
cs = 1.;
sn = 1.;
lp1 = l + 1;
i__2 = k;
for (i__ = lp1; i__ <= i__2; ++i__) {
g = e[i__];
y = q[i__];
h__ = sn * g;
g = cs * g;
g1_(&f, &h__, &cs, &sn, &e[i__ - 1]);
f = x * cs + g * sn;
g = -x * sn + g * cs;
h__ = y * sn;
y *= cs;
if (wntv) {
/* ACCUMULATE ROTATIONS (FROM THE RIGHT) IN 'V' */
i__3 = *nrv;
for (j = 1; j <= i__3; ++j) {
/* Apply procedure G2 (CS,SN,V(J,I-1),V(J,I)) */
temp = v[j + (i__ - 1) * v_dim1];
v[j + (i__ - 1) * v_dim1] = cs * temp + sn * v[j + i__ *
v_dim1];
v[j + i__ * v_dim1] = -sn * temp + cs * v[j + i__ *
v_dim1];
/* L130: */
}
}
g1_(&f, &h__, &cs, &sn, &q[i__ - 1]);
f = cs * g + sn * y;
x = -sn * g + cs * y;
if (havers) {
i__3 = *ncc;
for (j = 1; j <= i__3; ++j) {
/* Apply procedure G2 (CS,SN,C(I-1,J),C(I,J)) */
temp = c__[i__ - 1 + j * c_dim1];
c__[i__ - 1 + j * c_dim1] = cs * temp + sn * c__[i__ + j *
c_dim1];
c__[i__ + j * c_dim1] = -sn * temp + cs * c__[i__ + j *
c_dim1];
/* L150: */
}
}
/* APPLY ROTATIONS FROM THE LEFT TO */
/* RIGHT HAND SIDES IN 'C'.. */
/* L160: */
}
e[l] = 0.;
e[k] = f;
q[k] = x;
++nqrs;
if (nqrs <= n10) {
goto L20;
}
/* RETURN TO 'TEST FOR SPLITTING'. */
small = (d__1 = e[k], abs(d__1));
i__ = k;
/* IF FAILURE TO CONVERGE SET SMALLEST MAGNITUDE */
/* TERM IN OFF-DIAGONAL TO ZERO. CONTINUE ON. */
/* .. */
i__2 = k;
for (j = l; j <= i__2; ++j) {
temp = (d__1 = e[j], abs(d__1));
if (temp == 0.) {
goto L165;
}
if (temp < small) {
small = temp;
i__ = j;
}
L165:
;
}
e[i__] = 0.;
nqrs = 0;
fail = TRUE_;
goto L20;
/* .. */
/* CUTOFF FOR CONVERGENCE FAILURE. 'NQRS' WILL BE 2*N USUALLY. */
L170:
if (z__ >= 0.) {
goto L190;
}
q[k] = -z__;
if (wntv) {
i__2 = *nrv;
for (j = 1; j <= i__2; ++j) {
/* L180: */
v[j + k * v_dim1] = -v[j + k * v_dim1];
}
}
L190:
/* CONVERGENCE. Q(K) IS MADE NONNEGATIVE.. */
/* L200: */
;
}
if (n == 1) {
return 0;
}
i__1 = n;
for (i__ = 2; i__ <= i__1; ++i__) {
if (q[i__] > q[i__ - 1]) {
goto L220;
}
/* L210: */
}
if (fail) {
*ipass = 2;
}
return 0;
/* .. */
/* EVERY SINGULAR VALUE IS IN ORDER.. */
L220:
i__1 = n;
for (i__ = 2; i__ <= i__1; ++i__) {
t = q[i__ - 1];
k = i__ - 1;
i__2 = n;
for (j = i__; j <= i__2; ++j) {
if (t >= q[j]) {
goto L230;
}
t = q[j];
k = j;
L230:
;
}
if (k == i__ - 1) {
goto L270;
}
q[k] = q[i__ - 1];
q[i__ - 1] = t;
if (havers) {
i__2 = *ncc;
for (j = 1; j <= i__2; ++j) {
t = c__[i__ - 1 + j * c_dim1];
c__[i__ - 1 + j * c_dim1] = c__[k + j * c_dim1];
/* L240: */
c__[k + j * c_dim1] = t;
}
}
/* L250: */
if (wntv) {
i__2 = *nrv;
for (j = 1; j <= i__2; ++j) {
t = v[j + (i__ - 1) * v_dim1];
v[j + (i__ - 1) * v_dim1] = v[j + k * v_dim1];
/* L260: */
v[j + k * v_dim1] = t;
}
}
L270:
;
}
/* END OF ORDERING ALGORITHM. */
if (fail) {
*ipass = 2;
}
return 0;
} /* qrbd_ */
