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_ */