Beispiel #1
0
static void  TRAN_Change_axis_Grid(
   int n1, int n2, int n3,
   double  *grid)
   /* order n2, n3, n1 */
#define v_ref(i,j,k)  v[  (i)*n3*n1+(j)*n1+k ]
#define grid_ref(i,j,k)  grid[ (i)*n2*n3+(j)*n3+k ]
{

  int i,j,k;
  double *v;

  v = (double*)malloc(sizeof(double)*n1*n2*n3);

  for (i=0;i<n1*n2*n3;i++) v[i]=grid[i];

  for (i=0;i<n1;i++) {
   for (j=0;j<n2;j++) {
     for (k=0;k<n3;k++) {
       grid_ref(i,j,k) = v_ref(j,k,i);
     }
    }
  }
 
  free(v); 

}
BOOST_AUTO_TEST_CASE_TEMPLATE( iota, DeviceType, DTK_SEARCH_DEVICE_TYPES )
{
    int const n = 10;
    double const val = 3.;
    Kokkos::View<double *, DeviceType> v( "v", n );
    ArborX::iota( v, val );
    std::vector<double> v_ref( n );
    std::iota( v_ref.begin(), v_ref.end(), val );
    auto v_host = Kokkos::create_mirror_view( v );
    Kokkos::deep_copy( v_host, v );
    BOOST_TEST( v_ref == v_host, tt::per_element() );

    Kokkos::View<int[3], DeviceType> w( "w" );
    ArborX::iota( w );
    std::vector<int> w_ref = {0, 1, 2};
    auto w_host = Kokkos::create_mirror_view( w );
    Kokkos::deep_copy( w_host, w );
    BOOST_TEST( w_ref == w_host, tt::per_element() );
}
Beispiel #3
0
void test_nmf(std::size_t m, std::size_t k, std::size_t n)
{
    std::vector<ScalarType> stl_w(m * k);
    std::vector<ScalarType> stl_h(k * n);

    viennacl::matrix<ScalarType> v_ref(m, n);
    viennacl::matrix<ScalarType> w_ref(m, k);
    viennacl::matrix<ScalarType> h_ref(k, n);

    fill_random(stl_w);
    fill_random(stl_h);

    viennacl::fast_copy(&stl_w[0], &stl_w[0] + stl_w.size(), w_ref);
    viennacl::fast_copy(&stl_h[0], &stl_h[0] + stl_h.size(), h_ref);

    v_ref = viennacl::linalg::prod(w_ref, h_ref);  //reference

    // Fill again with random numbers:
    fill_random(stl_w);
    fill_random(stl_h);

    viennacl::matrix<ScalarType> w_nmf(m, k);
    viennacl::matrix<ScalarType> h_nmf(k, n);

    viennacl::fast_copy(&stl_w[0], &stl_w[0] + stl_w.size(), w_nmf);
    viennacl::fast_copy(&stl_h[0], &stl_h[0] + stl_h.size(), h_nmf);



    viennacl::linalg::nmf_config conf;
    viennacl::linalg::nmf(v_ref, w_nmf, h_nmf, conf);

    viennacl::matrix<ScalarType> v_nmf = viennacl::linalg::prod(w_nmf, h_nmf);

    float diff  = matrix_compare(v_ref, v_nmf);
    bool diff_ok = fabs(diff) < EPS;

    long iterations = static_cast<long>(conf.iters());
    printf("%6s [%lux%lux%lu] diff = %.6f (%ld iterations)\n", diff_ok ? "[[OK]]":"[FAIL]", m, k, n, diff, iterations);

    if (!diff_ok)
      exit(EXIT_FAILURE);
}
Beispiel #4
0
/* Subroutine */ int dgebak_(char *job, char *side, integer *n, integer *ilo, 
	integer *ihi, doublereal *scale, integer *m, doublereal *v, integer *
	ldv, integer *info)
{
/*  -- LAPACK routine (version 3.0) --   
       Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,   
       Courant Institute, Argonne National Lab, and Rice University   
       September 30, 1994   


    Purpose   
    =======   

    DGEBAK forms the right or left eigenvectors of a real general matrix   
    by backward transformation on the computed eigenvectors of the   
    balanced matrix output by DGEBAL.   

    Arguments   
    =========   

    JOB     (input) CHARACTER*1   
            Specifies the type of backward transformation required:   
            = 'N', do nothing, return immediately;   
            = 'P', do backward transformation for permutation only;   
            = 'S', do backward transformation for scaling only;   
            = 'B', do backward transformations for both permutation and   
                   scaling.   
            JOB must be the same as the argument JOB supplied to DGEBAL.   

    SIDE    (input) CHARACTER*1   
            = 'R':  V contains right eigenvectors;   
            = 'L':  V contains left eigenvectors.   

    N       (input) INTEGER   
            The number of rows of the matrix V.  N >= 0.   

    ILO     (input) INTEGER   
    IHI     (input) INTEGER   
            The integers ILO and IHI determined by DGEBAL.   
            1 <= ILO <= IHI <= N, if N > 0; ILO=1 and IHI=0, if N=0.   

    SCALE   (input) DOUBLE PRECISION array, dimension (N)   
            Details of the permutation and scaling factors, as returned   
            by DGEBAL.   

    M       (input) INTEGER   
            The number of columns of the matrix V.  M >= 0.   

    V       (input/output) DOUBLE PRECISION array, dimension (LDV,M)   
            On entry, the matrix of right or left eigenvectors to be   
            transformed, as returned by DHSEIN or DTREVC.   
            On exit, V is overwritten by the transformed eigenvectors.   

    LDV     (input) INTEGER   
            The leading dimension of the array V. LDV >= max(1,N).   

    INFO    (output) INTEGER   
            = 0:  successful exit   
            < 0:  if INFO = -i, the i-th argument had an illegal value.   

    =====================================================================   


       Decode and Test the input parameters   

       Parameter adjustments */
    /* System generated locals */
    integer v_dim1, v_offset, i__1;
    /* Local variables */
    static integer i__, k;
    static doublereal s;
    extern /* Subroutine */ int dscal_(integer *, doublereal *, doublereal *, 
	    integer *);
    extern logical lsame_(char *, char *);
    extern /* Subroutine */ int dswap_(integer *, doublereal *, integer *, 
	    doublereal *, integer *);
    static logical leftv;
    static integer ii;
    extern /* Subroutine */ int xerbla_(char *, integer *);
    static logical rightv;
#define v_ref(a_1,a_2) v[(a_2)*v_dim1 + a_1]

    --scale;
    v_dim1 = *ldv;
    v_offset = 1 + v_dim1 * 1;
    v -= v_offset;

    /* Function Body */
    rightv = lsame_(side, "R");
    leftv = lsame_(side, "L");

    *info = 0;
    if (! lsame_(job, "N") && ! lsame_(job, "P") && ! lsame_(job, "S") 
	    && ! lsame_(job, "B")) {
	*info = -1;
    } else if (! rightv && ! leftv) {
	*info = -2;
    } else if (*n < 0) {
	*info = -3;
    } else if (*ilo < 1 || *ilo > max(1,*n)) {
	*info = -4;
    } else if (*ihi < min(*ilo,*n) || *ihi > *n) {
	*info = -5;
    } else if (*m < 0) {
	*info = -7;
    } else if (*ldv < max(1,*n)) {
	*info = -9;
    }
    if (*info != 0) {
	i__1 = -(*info);
	xerbla_("DGEBAK", &i__1);
	return 0;
    }

/*     Quick return if possible */

    if (*n == 0) {
	return 0;
    }
    if (*m == 0) {
	return 0;
    }
    if (lsame_(job, "N")) {
	return 0;
    }

    if (*ilo == *ihi) {
	goto L30;
    }

/*     Backward balance */

    if (lsame_(job, "S") || lsame_(job, "B")) {

	if (rightv) {
	    i__1 = *ihi;
	    for (i__ = *ilo; i__ <= i__1; ++i__) {
		s = scale[i__];
		dscal_(m, &s, &v_ref(i__, 1), ldv);
/* L10: */
	    }
	}

	if (leftv) {
	    i__1 = *ihi;
	    for (i__ = *ilo; i__ <= i__1; ++i__) {
		s = 1. / scale[i__];
		dscal_(m, &s, &v_ref(i__, 1), ldv);
/* L20: */
	    }
	}

    }

/*     Backward permutation   

       For  I = ILO-1 step -1 until 1,   
                IHI+1 step 1 until N do -- */

L30:
    if (lsame_(job, "P") || lsame_(job, "B")) {
	if (rightv) {
	    i__1 = *n;
	    for (ii = 1; ii <= i__1; ++ii) {
		i__ = ii;
		if (i__ >= *ilo && i__ <= *ihi) {
		    goto L40;
		}
		if (i__ < *ilo) {
		    i__ = *ilo - ii;
		}
		k = (integer) scale[i__];
		if (k == i__) {
		    goto L40;
		}
		dswap_(m, &v_ref(i__, 1), ldv, &v_ref(k, 1), ldv);
L40:
		;
	    }
	}

	if (leftv) {
	    i__1 = *n;
	    for (ii = 1; ii <= i__1; ++ii) {
		i__ = ii;
		if (i__ >= *ilo && i__ <= *ihi) {
		    goto L50;
		}
		if (i__ < *ilo) {
		    i__ = *ilo - ii;
		}
		k = (integer) scale[i__];
		if (k == i__) {
		    goto L50;
		}
		dswap_(m, &v_ref(i__, 1), ldv, &v_ref(k, 1), ldv);
L50:
		;
	    }
	}
    }

    return 0;

/*     End of DGEBAK */

} /* dgebak_ */
/* Subroutine */ int cggsvp_(char *jobu, char *jobv, char *jobq, integer *m, 
	integer *p, integer *n, complex *a, integer *lda, complex *b, integer 
	*ldb, real *tola, real *tolb, integer *k, integer *l, complex *u, 
	integer *ldu, complex *v, integer *ldv, complex *q, integer *ldq, 
	integer *iwork, real *rwork, complex *tau, complex *work, integer *
	info)
{
/*  -- LAPACK routine (version 3.0) --   
       Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,   
       Courant Institute, Argonne National Lab, and Rice University   
       September 30, 1994   


    Purpose   
    =======   

    CGGSVP computes unitary matrices U, V and Q such that   

                     N-K-L  K    L   
     U'*A*Q =     K ( 0    A12  A13 )  if M-K-L >= 0;   
                  L ( 0     0   A23 )   
              M-K-L ( 0     0    0  )   

                     N-K-L  K    L   
            =     K ( 0    A12  A13 )  if M-K-L < 0;   
                M-K ( 0     0   A23 )   

                   N-K-L  K    L   
     V'*B*Q =   L ( 0     0   B13 )   
              P-L ( 0     0    0  )   

    where the K-by-K matrix A12 and L-by-L matrix B13 are nonsingular   
    upper triangular; A23 is L-by-L upper triangular if M-K-L >= 0,   
    otherwise A23 is (M-K)-by-L upper trapezoidal.  K+L = the effective   
    numerical rank of the (M+P)-by-N matrix (A',B')'.  Z' denotes the   
    conjugate transpose of Z.   

    This decomposition is the preprocessing step for computing the   
    Generalized Singular Value Decomposition (GSVD), see subroutine   
    CGGSVD.   

    Arguments   
    =========   

    JOBU    (input) CHARACTER*1   
            = 'U':  Unitary matrix U is computed;   
            = 'N':  U is not computed.   

    JOBV    (input) CHARACTER*1   
            = 'V':  Unitary matrix V is computed;   
            = 'N':  V is not computed.   

    JOBQ    (input) CHARACTER*1   
            = 'Q':  Unitary matrix Q is computed;   
            = 'N':  Q is not computed.   

    M       (input) INTEGER   
            The number of rows of the matrix A.  M >= 0.   

    P       (input) INTEGER   
            The number of rows of the matrix B.  P >= 0.   

    N       (input) INTEGER   
            The number of columns of the matrices A and B.  N >= 0.   

    A       (input/output) COMPLEX array, dimension (LDA,N)   
            On entry, the M-by-N matrix A.   
            On exit, A contains the triangular (or trapezoidal) matrix   
            described in the Purpose section.   

    LDA     (input) INTEGER   
            The leading dimension of the array A. LDA >= max(1,M).   

    B       (input/output) COMPLEX array, dimension (LDB,N)   
            On entry, the P-by-N matrix B.   
            On exit, B contains the triangular matrix described in   
            the Purpose section.   

    LDB     (input) INTEGER   
            The leading dimension of the array B. LDB >= max(1,P).   

    TOLA    (input) REAL   
    TOLB    (input) REAL   
            TOLA and TOLB are the thresholds to determine the effective   
            numerical rank of matrix B and a subblock of A. Generally,   
            they are set to   
               TOLA = MAX(M,N)*norm(A)*MACHEPS,   
               TOLB = MAX(P,N)*norm(B)*MACHEPS.   
            The size of TOLA and TOLB may affect the size of backward   
            errors of the decomposition.   

    K       (output) INTEGER   
    L       (output) INTEGER   
            On exit, K and L specify the dimension of the subblocks   
            described in Purpose section.   
            K + L = effective numerical rank of (A',B')'.   

    U       (output) COMPLEX array, dimension (LDU,M)   
            If JOBU = 'U', U contains the unitary matrix U.   
            If JOBU = 'N', U is not referenced.   

    LDU     (input) INTEGER   
            The leading dimension of the array U. LDU >= max(1,M) if   
            JOBU = 'U'; LDU >= 1 otherwise.   

    V       (output) COMPLEX array, dimension (LDV,M)   
            If JOBV = 'V', V contains the unitary matrix V.   
            If JOBV = 'N', V is not referenced.   

    LDV     (input) INTEGER   
            The leading dimension of the array V. LDV >= max(1,P) if   
            JOBV = 'V'; LDV >= 1 otherwise.   

    Q       (output) COMPLEX array, dimension (LDQ,N)   
            If JOBQ = 'Q', Q contains the unitary matrix Q.   
            If JOBQ = 'N', Q is not referenced.   

    LDQ     (input) INTEGER   
            The leading dimension of the array Q. LDQ >= max(1,N) if   
            JOBQ = 'Q'; LDQ >= 1 otherwise.   

    IWORK   (workspace) INTEGER array, dimension (N)   

    RWORK   (workspace) REAL array, dimension (2*N)   

    TAU     (workspace) COMPLEX array, dimension (N)   

    WORK    (workspace) COMPLEX array, dimension (max(3*N,M,P))   

    INFO    (output) INTEGER   
            = 0:  successful exit   
            < 0:  if INFO = -i, the i-th argument had an illegal value.   

    Further Details   
    ===============   

    The subroutine uses LAPACK subroutine CGEQPF for the QR factorization   
    with column pivoting to detect the effective numerical rank of the   
    a matrix. It may be replaced by a better rank determination strategy.   

    =====================================================================   


       Test the input parameters   

       Parameter adjustments */
    /* Table of constant values */
    static complex c_b1 = {0.f,0.f};
    static complex c_b2 = {1.f,0.f};
    
    /* System generated locals */
    integer a_dim1, a_offset, b_dim1, b_offset, q_dim1, q_offset, u_dim1, 
	    u_offset, v_dim1, v_offset, i__1, i__2, i__3;
    real r__1, r__2;
    /* Builtin functions */
    double r_imag(complex *);
    /* Local variables */
    static integer i__, j;
    extern logical lsame_(char *, char *);
    static logical wantq, wantu, wantv;
    extern /* Subroutine */ int cgeqr2_(integer *, integer *, complex *, 
	    integer *, complex *, complex *, integer *), cgerq2_(integer *, 
	    integer *, complex *, integer *, complex *, complex *, integer *),
	     cung2r_(integer *, integer *, integer *, complex *, integer *, 
	    complex *, complex *, integer *), cunm2r_(char *, char *, integer 
	    *, integer *, integer *, complex *, integer *, complex *, complex 
	    *, integer *, complex *, integer *), cunmr2_(char 
	    *, char *, integer *, integer *, integer *, complex *, integer *, 
	    complex *, complex *, integer *, complex *, integer *), cgeqpf_(integer *, integer *, complex *, integer *, 
	    integer *, complex *, complex *, real *, integer *), clacpy_(char 
	    *, integer *, integer *, complex *, integer *, complex *, integer 
	    *), claset_(char *, integer *, integer *, complex *, 
	    complex *, complex *, integer *), xerbla_(char *, integer 
	    *), clapmt_(logical *, integer *, integer *, complex *, 
	    integer *, integer *);
    static logical forwrd;
#define a_subscr(a_1,a_2) (a_2)*a_dim1 + a_1
#define a_ref(a_1,a_2) a[a_subscr(a_1,a_2)]
#define b_subscr(a_1,a_2) (a_2)*b_dim1 + a_1
#define b_ref(a_1,a_2) b[b_subscr(a_1,a_2)]
#define u_subscr(a_1,a_2) (a_2)*u_dim1 + a_1
#define u_ref(a_1,a_2) u[u_subscr(a_1,a_2)]
#define v_subscr(a_1,a_2) (a_2)*v_dim1 + a_1
#define v_ref(a_1,a_2) v[v_subscr(a_1,a_2)]


    a_dim1 = *lda;
    a_offset = 1 + a_dim1 * 1;
    a -= a_offset;
    b_dim1 = *ldb;
    b_offset = 1 + b_dim1 * 1;
    b -= b_offset;
    u_dim1 = *ldu;
    u_offset = 1 + u_dim1 * 1;
    u -= u_offset;
    v_dim1 = *ldv;
    v_offset = 1 + v_dim1 * 1;
    v -= v_offset;
    q_dim1 = *ldq;
    q_offset = 1 + q_dim1 * 1;
    q -= q_offset;
    --iwork;
    --rwork;
    --tau;
    --work;

    /* Function Body */
    wantu = lsame_(jobu, "U");
    wantv = lsame_(jobv, "V");
    wantq = lsame_(jobq, "Q");
    forwrd = TRUE_;

    *info = 0;
    if (! (wantu || lsame_(jobu, "N"))) {
	*info = -1;
    } else if (! (wantv || lsame_(jobv, "N"))) {
	*info = -2;
    } else if (! (wantq || lsame_(jobq, "N"))) {
	*info = -3;
    } else if (*m < 0) {
	*info = -4;
    } else if (*p < 0) {
	*info = -5;
    } else if (*n < 0) {
	*info = -6;
    } else if (*lda < max(1,*m)) {
	*info = -8;
    } else if (*ldb < max(1,*p)) {
	*info = -10;
    } else if (*ldu < 1 || wantu && *ldu < *m) {
	*info = -16;
    } else if (*ldv < 1 || wantv && *ldv < *p) {
	*info = -18;
    } else if (*ldq < 1 || wantq && *ldq < *n) {
	*info = -20;
    }
    if (*info != 0) {
	i__1 = -(*info);
	xerbla_("CGGSVP", &i__1);
	return 0;
    }

/*     QR with column pivoting of B: B*P = V*( S11 S12 )   
                                             (  0   0  ) */

    i__1 = *n;
    for (i__ = 1; i__ <= i__1; ++i__) {
	iwork[i__] = 0;
/* L10: */
    }
    cgeqpf_(p, n, &b[b_offset], ldb, &iwork[1], &tau[1], &work[1], &rwork[1], 
	    info);

/*     Update A := A*P */

    clapmt_(&forwrd, m, n, &a[a_offset], lda, &iwork[1]);

/*     Determine the effective rank of matrix B. */

    *l = 0;
    i__1 = min(*p,*n);
    for (i__ = 1; i__ <= i__1; ++i__) {
	i__2 = b_subscr(i__, i__);
	if ((r__1 = b[i__2].r, dabs(r__1)) + (r__2 = r_imag(&b_ref(i__, i__)),
		 dabs(r__2)) > *tolb) {
	    ++(*l);
	}
/* L20: */
    }

    if (wantv) {

/*        Copy the details of V, and form V. */

	claset_("Full", p, p, &c_b1, &c_b1, &v[v_offset], ldv);
	if (*p > 1) {
	    i__1 = *p - 1;
	    clacpy_("Lower", &i__1, n, &b_ref(2, 1), ldb, &v_ref(2, 1), ldv);
	}
	i__1 = min(*p,*n);
	cung2r_(p, p, &i__1, &v[v_offset], ldv, &tau[1], &work[1], info);
    }

/*     Clean up B */

    i__1 = *l - 1;
    for (j = 1; j <= i__1; ++j) {
	i__2 = *l;
	for (i__ = j + 1; i__ <= i__2; ++i__) {
	    i__3 = b_subscr(i__, j);
	    b[i__3].r = 0.f, b[i__3].i = 0.f;
/* L30: */
	}
/* L40: */
    }
    if (*p > *l) {
	i__1 = *p - *l;
	claset_("Full", &i__1, n, &c_b1, &c_b1, &b_ref(*l + 1, 1), ldb);
    }

    if (wantq) {

/*        Set Q = I and Update Q := Q*P */

	claset_("Full", n, n, &c_b1, &c_b2, &q[q_offset], ldq);
	clapmt_(&forwrd, n, n, &q[q_offset], ldq, &iwork[1]);
    }

    if (*p >= *l && *n != *l) {

/*        RQ factorization of ( S11 S12 ) = ( 0 S12 )*Z */

	cgerq2_(l, n, &b[b_offset], ldb, &tau[1], &work[1], info);

/*        Update A := A*Z' */

	cunmr2_("Right", "Conjugate transpose", m, n, l, &b[b_offset], ldb, &
		tau[1], &a[a_offset], lda, &work[1], info);
	if (wantq) {

/*           Update Q := Q*Z' */

	    cunmr2_("Right", "Conjugate transpose", n, n, l, &b[b_offset], 
		    ldb, &tau[1], &q[q_offset], ldq, &work[1], info);
	}

/*        Clean up B */

	i__1 = *n - *l;
	claset_("Full", l, &i__1, &c_b1, &c_b1, &b[b_offset], ldb);
	i__1 = *n;
	for (j = *n - *l + 1; j <= i__1; ++j) {
	    i__2 = *l;
	    for (i__ = j - *n + *l + 1; i__ <= i__2; ++i__) {
		i__3 = b_subscr(i__, j);
		b[i__3].r = 0.f, b[i__3].i = 0.f;
/* L50: */
	    }
/* L60: */
	}

    }

/*     Let              N-L     L   
                  A = ( A11    A12 ) M,   

       then the following does the complete QR decomposition of A11:   

                A11 = U*(  0  T12 )*P1'   
                        (  0   0  ) */

    i__1 = *n - *l;
    for (i__ = 1; i__ <= i__1; ++i__) {
	iwork[i__] = 0;
/* L70: */
    }
    i__1 = *n - *l;
    cgeqpf_(m, &i__1, &a[a_offset], lda, &iwork[1], &tau[1], &work[1], &rwork[
	    1], info);

/*     Determine the effective rank of A11 */

    *k = 0;
/* Computing MIN */
    i__2 = *m, i__3 = *n - *l;
    i__1 = min(i__2,i__3);
    for (i__ = 1; i__ <= i__1; ++i__) {
	i__2 = a_subscr(i__, i__);
	if ((r__1 = a[i__2].r, dabs(r__1)) + (r__2 = r_imag(&a_ref(i__, i__)),
		 dabs(r__2)) > *tola) {
	    ++(*k);
	}
/* L80: */
    }

/*     Update A12 := U'*A12, where A12 = A( 1:M, N-L+1:N )   

   Computing MIN */
    i__2 = *m, i__3 = *n - *l;
    i__1 = min(i__2,i__3);
    cunm2r_("Left", "Conjugate transpose", m, l, &i__1, &a[a_offset], lda, &
	    tau[1], &a_ref(1, *n - *l + 1), lda, &work[1], info);

    if (wantu) {

/*        Copy the details of U, and form U */

	claset_("Full", m, m, &c_b1, &c_b1, &u[u_offset], ldu);
	if (*m > 1) {
	    i__1 = *m - 1;
	    i__2 = *n - *l;
	    clacpy_("Lower", &i__1, &i__2, &a_ref(2, 1), lda, &u_ref(2, 1), 
		    ldu);
	}
/* Computing MIN */
	i__2 = *m, i__3 = *n - *l;
	i__1 = min(i__2,i__3);
	cung2r_(m, m, &i__1, &u[u_offset], ldu, &tau[1], &work[1], info);
    }

    if (wantq) {

/*        Update Q( 1:N, 1:N-L )  = Q( 1:N, 1:N-L )*P1 */

	i__1 = *n - *l;
	clapmt_(&forwrd, n, &i__1, &q[q_offset], ldq, &iwork[1]);
    }

/*     Clean up A: set the strictly lower triangular part of   
       A(1:K, 1:K) = 0, and A( K+1:M, 1:N-L ) = 0. */

    i__1 = *k - 1;
    for (j = 1; j <= i__1; ++j) {
	i__2 = *k;
	for (i__ = j + 1; i__ <= i__2; ++i__) {
	    i__3 = a_subscr(i__, j);
	    a[i__3].r = 0.f, a[i__3].i = 0.f;
/* L90: */
	}
/* L100: */
    }
    if (*m > *k) {
	i__1 = *m - *k;
	i__2 = *n - *l;
	claset_("Full", &i__1, &i__2, &c_b1, &c_b1, &a_ref(*k + 1, 1), lda);
    }

    if (*n - *l > *k) {

/*        RQ factorization of ( T11 T12 ) = ( 0 T12 )*Z1 */

	i__1 = *n - *l;
	cgerq2_(k, &i__1, &a[a_offset], lda, &tau[1], &work[1], info);

	if (wantq) {

/*           Update Q( 1:N,1:N-L ) = Q( 1:N,1:N-L )*Z1' */

	    i__1 = *n - *l;
	    cunmr2_("Right", "Conjugate transpose", n, &i__1, k, &a[a_offset],
		     lda, &tau[1], &q[q_offset], ldq, &work[1], info);
	}

/*        Clean up A */

	i__1 = *n - *l - *k;
	claset_("Full", k, &i__1, &c_b1, &c_b1, &a[a_offset], lda);
	i__1 = *n - *l;
	for (j = *n - *l - *k + 1; j <= i__1; ++j) {
	    i__2 = *k;
	    for (i__ = j - *n + *l + *k + 1; i__ <= i__2; ++i__) {
		i__3 = a_subscr(i__, j);
		a[i__3].r = 0.f, a[i__3].i = 0.f;
/* L110: */
	    }
/* L120: */
	}

    }

    if (*m > *k) {

/*        QR factorization of A( K+1:M,N-L+1:N ) */

	i__1 = *m - *k;
	cgeqr2_(&i__1, l, &a_ref(*k + 1, *n - *l + 1), lda, &tau[1], &work[1],
		 info);

	if (wantu) {

/*           Update U(:,K+1:M) := U(:,K+1:M)*U1 */

	    i__1 = *m - *k;
/* Computing MIN */
	    i__3 = *m - *k;
	    i__2 = min(i__3,*l);
	    cunm2r_("Right", "No transpose", m, &i__1, &i__2, &a_ref(*k + 1, *
		    n - *l + 1), lda, &tau[1], &u_ref(1, *k + 1), ldu, &work[
		    1], info);
	}

/*        Clean up */

	i__1 = *n;
	for (j = *n - *l + 1; j <= i__1; ++j) {
	    i__2 = *m;
	    for (i__ = j - *n + *k + *l + 1; i__ <= i__2; ++i__) {
		i__3 = a_subscr(i__, j);
		a[i__3].r = 0.f, a[i__3].i = 0.f;
/* L130: */
	    }
/* L140: */
	}

    }

    return 0;

/*     End of CGGSVP */

} /* cggsvp_ */
/* Subroutine */ int dggbak_(char *job, char *side, integer *n, integer *ilo, 
	integer *ihi, doublereal *lscale, doublereal *rscale, integer *m, 
	doublereal *v, integer *ldv, integer *info)
{
/*  -- LAPACK routine (version 3.0) --   
       Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,   
       Courant Institute, Argonne National Lab, and Rice University   
       September 30, 1994   


    Purpose   
    =======   

    DGGBAK forms the right or left eigenvectors of a real generalized   
    eigenvalue problem A*x = lambda*B*x, by backward transformation on   
    the computed eigenvectors of the balanced pair of matrices output by   
    DGGBAL.   

    Arguments   
    =========   

    JOB     (input) CHARACTER*1   
            Specifies the type of backward transformation required:   
            = 'N':  do nothing, return immediately;   
            = 'P':  do backward transformation for permutation only;   
            = 'S':  do backward transformation for scaling only;   
            = 'B':  do backward transformations for both permutation and   
                    scaling.   
            JOB must be the same as the argument JOB supplied to DGGBAL.   

    SIDE    (input) CHARACTER*1   
            = 'R':  V contains right eigenvectors;   
            = 'L':  V contains left eigenvectors.   

    N       (input) INTEGER   
            The number of rows of the matrix V.  N >= 0.   

    ILO     (input) INTEGER   
    IHI     (input) INTEGER   
            The integers ILO and IHI determined by DGGBAL.   
            1 <= ILO <= IHI <= N, if N > 0; ILO=1 and IHI=0, if N=0.   

    LSCALE  (input) DOUBLE PRECISION array, dimension (N)   
            Details of the permutations and/or scaling factors applied   
            to the left side of A and B, as returned by DGGBAL.   

    RSCALE  (input) DOUBLE PRECISION array, dimension (N)   
            Details of the permutations and/or scaling factors applied   
            to the right side of A and B, as returned by DGGBAL.   

    M       (input) INTEGER   
            The number of columns of the matrix V.  M >= 0.   

    V       (input/output) DOUBLE PRECISION array, dimension (LDV,M)   
            On entry, the matrix of right or left eigenvectors to be   
            transformed, as returned by DTGEVC.   
            On exit, V is overwritten by the transformed eigenvectors.   

    LDV     (input) INTEGER   
            The leading dimension of the matrix V. LDV >= max(1,N).   

    INFO    (output) INTEGER   
            = 0:  successful exit.   
            < 0:  if INFO = -i, the i-th argument had an illegal value.   

    Further Details   
    ===============   

    See R.C. Ward, Balancing the generalized eigenvalue problem,   
                   SIAM J. Sci. Stat. Comp. 2 (1981), 141-152.   

    =====================================================================   


       Test the input parameters   

       Parameter adjustments */
    /* System generated locals */
    integer v_dim1, v_offset, i__1;
    /* Local variables */
    static integer i__, k;
    extern /* Subroutine */ int dscal_(integer *, doublereal *, doublereal *, 
	    integer *);
    extern logical lsame_(char *, char *);
    extern /* Subroutine */ int dswap_(integer *, doublereal *, integer *, 
	    doublereal *, integer *);
    static logical leftv;
    extern /* Subroutine */ int xerbla_(char *, integer *);
    static logical rightv;
#define v_ref(a_1,a_2) v[(a_2)*v_dim1 + a_1]

    --lscale;
    --rscale;
    v_dim1 = *ldv;
    v_offset = 1 + v_dim1 * 1;
    v -= v_offset;

    /* Function Body */
    rightv = lsame_(side, "R");
    leftv = lsame_(side, "L");

    *info = 0;
    if (! lsame_(job, "N") && ! lsame_(job, "P") && ! lsame_(job, "S") 
	    && ! lsame_(job, "B")) {
	*info = -1;
    } else if (! rightv && ! leftv) {
	*info = -2;
    } else if (*n < 0) {
	*info = -3;
    } else if (*ilo < 1) {
	*info = -4;
    } else if (*ihi < *ilo || *ihi > max(1,*n)) {
	*info = -5;
    } else if (*m < 0) {
	*info = -6;
    } else if (*ldv < max(1,*n)) {
	*info = -10;
    }
    if (*info != 0) {
	i__1 = -(*info);
	xerbla_("DGGBAK", &i__1);
	return 0;
    }

/*     Quick return if possible */

    if (*n == 0) {
	return 0;
    }
    if (*m == 0) {
	return 0;
    }
    if (lsame_(job, "N")) {
	return 0;
    }

    if (*ilo == *ihi) {
	goto L30;
    }

/*     Backward balance */

    if (lsame_(job, "S") || lsame_(job, "B")) {

/*        Backward transformation on right eigenvectors */

	if (rightv) {
	    i__1 = *ihi;
	    for (i__ = *ilo; i__ <= i__1; ++i__) {
		dscal_(m, &rscale[i__], &v_ref(i__, 1), ldv);
/* L10: */
	    }
	}

/*        Backward transformation on left eigenvectors */

	if (leftv) {
	    i__1 = *ihi;
	    for (i__ = *ilo; i__ <= i__1; ++i__) {
		dscal_(m, &lscale[i__], &v_ref(i__, 1), ldv);
/* L20: */
	    }
	}
    }

/*     Backward permutation */

L30:
    if (lsame_(job, "P") || lsame_(job, "B")) {

/*        Backward permutation on right eigenvectors */

	if (rightv) {
	    if (*ilo == 1) {
		goto L50;
	    }

	    for (i__ = *ilo - 1; i__ >= 1; --i__) {
		k = (integer) rscale[i__];
		if (k == i__) {
		    goto L40;
		}
		dswap_(m, &v_ref(i__, 1), ldv, &v_ref(k, 1), ldv);
L40:
		;
	    }

L50:
	    if (*ihi == *n) {
		goto L70;
	    }
	    i__1 = *n;
	    for (i__ = *ihi + 1; i__ <= i__1; ++i__) {
		k = (integer) rscale[i__];
		if (k == i__) {
		    goto L60;
		}
		dswap_(m, &v_ref(i__, 1), ldv, &v_ref(k, 1), ldv);
L60:
		;
	    }
	}

/*        Backward permutation on left eigenvectors */

L70:
	if (leftv) {
	    if (*ilo == 1) {
		goto L90;
	    }
	    for (i__ = *ilo - 1; i__ >= 1; --i__) {
		k = (integer) lscale[i__];
		if (k == i__) {
		    goto L80;
		}
		dswap_(m, &v_ref(i__, 1), ldv, &v_ref(k, 1), ldv);
L80:
		;
	    }

L90:
	    if (*ihi == *n) {
		goto L110;
	    }
	    i__1 = *n;
	    for (i__ = *ihi + 1; i__ <= i__1; ++i__) {
		k = (integer) lscale[i__];
		if (k == i__) {
		    goto L100;
		}
		dswap_(m, &v_ref(i__, 1), ldv, &v_ref(k, 1), ldv);
L100:
		;
	    }
	}
    }

L110:

    return 0;

/*     End of DGGBAK */

} /* dggbak_ */
Beispiel #7
0
/* Subroutine */ int dlarft_(char *direct, char *storev, integer *n, integer *
	k, doublereal *v, integer *ldv, doublereal *tau, doublereal *t, 
	integer *ldt)
{
/*  -- LAPACK auxiliary routine (version 3.0) --   
       Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,   
       Courant Institute, Argonne National Lab, and Rice University   
       February 29, 1992   


    Purpose   
    =======   

    DLARFT forms the triangular factor T of a real block reflector H   
    of order n, which is defined as a product of k elementary reflectors.   

    If DIRECT = 'F', H = H(1) H(2) . . . H(k) and T is upper triangular;   

    If DIRECT = 'B', H = H(k) . . . H(2) H(1) and T is lower triangular.   

    If STOREV = 'C', the vector which defines the elementary reflector   
    H(i) is stored in the i-th column of the array V, and   

       H  =  I - V * T * V'   

    If STOREV = 'R', the vector which defines the elementary reflector   
    H(i) is stored in the i-th row of the array V, and   

       H  =  I - V' * T * V   

    Arguments   
    =========   

    DIRECT  (input) CHARACTER*1   
            Specifies the order in which the elementary reflectors are   
            multiplied to form the block reflector:   
            = 'F': H = H(1) H(2) . . . H(k) (Forward)   
            = 'B': H = H(k) . . . H(2) H(1) (Backward)   

    STOREV  (input) CHARACTER*1   
            Specifies how the vectors which define the elementary   
            reflectors are stored (see also Further Details):   
            = 'C': columnwise   
            = 'R': rowwise   

    N       (input) INTEGER   
            The order of the block reflector H. N >= 0.   

    K       (input) INTEGER   
            The order of the triangular factor T (= the number of   
            elementary reflectors). K >= 1.   

    V       (input/output) DOUBLE PRECISION array, dimension   
                                 (LDV,K) if STOREV = 'C'   
                                 (LDV,N) if STOREV = 'R'   
            The matrix V. See further details.   

    LDV     (input) INTEGER   
            The leading dimension of the array V.   
            If STOREV = 'C', LDV >= max(1,N); if STOREV = 'R', LDV >= K.   

    TAU     (input) DOUBLE PRECISION array, dimension (K)   
            TAU(i) must contain the scalar factor of the elementary   
            reflector H(i).   

    T       (output) DOUBLE PRECISION array, dimension (LDT,K)   
            The k by k triangular factor T of the block reflector.   
            If DIRECT = 'F', T is upper triangular; if DIRECT = 'B', T is   
            lower triangular. The rest of the array is not used.   

    LDT     (input) INTEGER   
            The leading dimension of the array T. LDT >= K.   

    Further Details   
    ===============   

    The shape of the matrix V and the storage of the vectors which define   
    the H(i) is best illustrated by the following example with n = 5 and   
    k = 3. The elements equal to 1 are not stored; the corresponding   
    array elements are modified but restored on exit. The rest of the   
    array is not used.   

    DIRECT = 'F' and STOREV = 'C':         DIRECT = 'F' and STOREV = 'R':   

                 V = (  1       )                 V = (  1 v1 v1 v1 v1 )   
                     ( v1  1    )                     (     1 v2 v2 v2 )   
                     ( v1 v2  1 )                     (        1 v3 v3 )   
                     ( v1 v2 v3 )   
                     ( v1 v2 v3 )   

    DIRECT = 'B' and STOREV = 'C':         DIRECT = 'B' and STOREV = 'R':   

                 V = ( v1 v2 v3 )                 V = ( v1 v1  1       )   
                     ( v1 v2 v3 )                     ( v2 v2 v2  1    )   
                     (  1 v2 v3 )                     ( v3 v3 v3 v3  1 )   
                     (     1 v3 )   
                     (        1 )   

    =====================================================================   


       Quick return if possible   

       Parameter adjustments */
    /* Table of constant values */
    static integer c__1 = 1;
    static doublereal c_b8 = 0.;
    
    /* System generated locals */
    integer t_dim1, t_offset, v_dim1, v_offset, i__1, i__2, i__3;
    doublereal d__1;
    /* Local variables */
    static integer i__, j;
    extern logical lsame_(char *, char *);
    extern /* Subroutine */ int dgemv_(char *, integer *, integer *, 
	    doublereal *, doublereal *, integer *, doublereal *, integer *, 
	    doublereal *, doublereal *, integer *), dtrmv_(char *, 
	    char *, char *, integer *, doublereal *, integer *, doublereal *, 
	    integer *);
    static doublereal vii;
#define t_ref(a_1,a_2) t[(a_2)*t_dim1 + a_1]
#define v_ref(a_1,a_2) v[(a_2)*v_dim1 + a_1]


    v_dim1 = *ldv;
    v_offset = 1 + v_dim1 * 1;
    v -= v_offset;
    --tau;
    t_dim1 = *ldt;
    t_offset = 1 + t_dim1 * 1;
    t -= t_offset;

    /* Function Body */
    if (*n == 0) {
	return 0;
    }

    if (lsame_(direct, "F")) {
	i__1 = *k;
	for (i__ = 1; i__ <= i__1; ++i__) {
	    if (tau[i__] == 0.) {

/*              H(i)  =  I */

		i__2 = i__;
		for (j = 1; j <= i__2; ++j) {
		    t_ref(j, i__) = 0.;
/* L10: */
		}
	    } else {

/*              general case */

		vii = v_ref(i__, i__);
		v_ref(i__, i__) = 1.;
		if (lsame_(storev, "C")) {

/*                 T(1:i-1,i) := - tau(i) * V(i:n,1:i-1)' * V(i:n,i) */

		    i__2 = *n - i__ + 1;
		    i__3 = i__ - 1;
		    d__1 = -tau[i__];
		    dgemv_("Transpose", &i__2, &i__3, &d__1, &v_ref(i__, 1), 
			    ldv, &v_ref(i__, i__), &c__1, &c_b8, &t_ref(1, 
			    i__), &c__1);
		} else {

/*                 T(1:i-1,i) := - tau(i) * V(1:i-1,i:n) * V(i,i:n)' */

		    i__2 = i__ - 1;
		    i__3 = *n - i__ + 1;
		    d__1 = -tau[i__];
		    dgemv_("No transpose", &i__2, &i__3, &d__1, &v_ref(1, i__)
			    , ldv, &v_ref(i__, i__), ldv, &c_b8, &t_ref(1, 
			    i__), &c__1);
		}
		v_ref(i__, i__) = vii;

/*              T(1:i-1,i) := T(1:i-1,1:i-1) * T(1:i-1,i) */

		i__2 = i__ - 1;
		dtrmv_("Upper", "No transpose", "Non-unit", &i__2, &t[
			t_offset], ldt, &t_ref(1, i__), &c__1);
		t_ref(i__, i__) = tau[i__];
	    }
/* L20: */
	}
    } else {
	for (i__ = *k; i__ >= 1; --i__) {
	    if (tau[i__] == 0.) {

/*              H(i)  =  I */

		i__1 = *k;
		for (j = i__; j <= i__1; ++j) {
		    t_ref(j, i__) = 0.;
/* L30: */
		}
	    } else {

/*              general case */

		if (i__ < *k) {
		    if (lsame_(storev, "C")) {
			vii = v_ref(*n - *k + i__, i__);
			v_ref(*n - *k + i__, i__) = 1.;

/*                    T(i+1:k,i) :=   
                              - tau(i) * V(1:n-k+i,i+1:k)' * V(1:n-k+i,i) */

			i__1 = *n - *k + i__;
			i__2 = *k - i__;
			d__1 = -tau[i__];
			dgemv_("Transpose", &i__1, &i__2, &d__1, &v_ref(1, 
				i__ + 1), ldv, &v_ref(1, i__), &c__1, &c_b8, &
				t_ref(i__ + 1, i__), &c__1);
			v_ref(*n - *k + i__, i__) = vii;
		    } else {
			vii = v_ref(i__, *n - *k + i__);
			v_ref(i__, *n - *k + i__) = 1.;

/*                    T(i+1:k,i) :=   
                              - tau(i) * V(i+1:k,1:n-k+i) * V(i,1:n-k+i)' */

			i__1 = *k - i__;
			i__2 = *n - *k + i__;
			d__1 = -tau[i__];
			dgemv_("No transpose", &i__1, &i__2, &d__1, &v_ref(
				i__ + 1, 1), ldv, &v_ref(i__, 1), ldv, &c_b8, 
				&t_ref(i__ + 1, i__), &c__1);
			v_ref(i__, *n - *k + i__) = vii;
		    }

/*                 T(i+1:k,i) := T(i+1:k,i+1:k) * T(i+1:k,i) */

		    i__1 = *k - i__;
		    dtrmv_("Lower", "No transpose", "Non-unit", &i__1, &t_ref(
			    i__ + 1, i__ + 1), ldt, &t_ref(i__ + 1, i__), &
			    c__1);
		}
		t_ref(i__, i__) = tau[i__];
	    }
/* L40: */
	}
    }
    return 0;

/*     End of DLARFT */

} /* dlarft_ */
Beispiel #8
0
/* Subroutine */ int dtgsja_(char *jobu, char *jobv, char *jobq, integer *m, 
	integer *p, integer *n, integer *k, integer *l, doublereal *a, 
	integer *lda, doublereal *b, integer *ldb, doublereal *tola, 
	doublereal *tolb, doublereal *alpha, doublereal *beta, doublereal *u, 
	integer *ldu, doublereal *v, integer *ldv, doublereal *q, integer *
	ldq, doublereal *work, integer *ncycle, integer *info)
{
/*  -- LAPACK routine (version 3.0) --   
       Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,   
       Courant Institute, Argonne National Lab, and Rice University   
       June 30, 1999   


    Purpose   
    =======   

    DTGSJA computes the generalized singular value decomposition (GSVD)   
    of two real upper triangular (or trapezoidal) matrices A and B.   

    On entry, it is assumed that matrices A and B have the following   
    forms, which may be obtained by the preprocessing subroutine DGGSVP   
    from a general M-by-N matrix A and P-by-N matrix B:   

                 N-K-L  K    L   
       A =    K ( 0    A12  A13 ) if M-K-L >= 0;   
              L ( 0     0   A23 )   
          M-K-L ( 0     0    0  )   

               N-K-L  K    L   
       A =  K ( 0    A12  A13 ) if M-K-L < 0;   
          M-K ( 0     0   A23 )   

               N-K-L  K    L   
       B =  L ( 0     0   B13 )   
          P-L ( 0     0    0  )   

    where the K-by-K matrix A12 and L-by-L matrix B13 are nonsingular   
    upper triangular; A23 is L-by-L upper triangular if M-K-L >= 0,   
    otherwise A23 is (M-K)-by-L upper trapezoidal.   

    On exit,   

                U'*A*Q = D1*( 0 R ),    V'*B*Q = D2*( 0 R ),   

    where U, V and Q are orthogonal matrices, Z' denotes the transpose   
    of Z, R is a nonsingular upper triangular matrix, and D1 and D2 are   
    ``diagonal'' matrices, which are of the following structures:   

    If M-K-L >= 0,   

                        K  L   
           D1 =     K ( I  0 )   
                    L ( 0  C )   
                M-K-L ( 0  0 )   

                      K  L   
           D2 = L   ( 0  S )   
                P-L ( 0  0 )   

                   N-K-L  K    L   
      ( 0 R ) = K (  0   R11  R12 ) K   
                L (  0    0   R22 ) L   

    where   

      C = diag( ALPHA(K+1), ... , ALPHA(K+L) ),   
      S = diag( BETA(K+1),  ... , BETA(K+L) ),   
      C**2 + S**2 = I.   

      R is stored in A(1:K+L,N-K-L+1:N) on exit.   

    If M-K-L < 0,   

                   K M-K K+L-M   
        D1 =   K ( I  0    0   )   
             M-K ( 0  C    0   )   

                     K M-K K+L-M   
        D2 =   M-K ( 0  S    0   )   
             K+L-M ( 0  0    I   )   
               P-L ( 0  0    0   )   

                   N-K-L  K   M-K  K+L-M   
   ( 0 R ) =    K ( 0    R11  R12  R13  )   
              M-K ( 0     0   R22  R23  )   
            K+L-M ( 0     0    0   R33  )   

    where   
    C = diag( ALPHA(K+1), ... , ALPHA(M) ),   
    S = diag( BETA(K+1),  ... , BETA(M) ),   
    C**2 + S**2 = I.   

    R = ( R11 R12 R13 ) is stored in A(1:M, N-K-L+1:N) and R33 is stored   
        (  0  R22 R23 )   
    in B(M-K+1:L,N+M-K-L+1:N) on exit.   

    The computation of the orthogonal transformation matrices U, V or Q   
    is optional.  These matrices may either be formed explicitly, or they   
    may be postmultiplied into input matrices U1, V1, or Q1.   

    Arguments   
    =========   

    JOBU    (input) CHARACTER*1   
            = 'U':  U must contain an orthogonal matrix U1 on entry, and   
                    the product U1*U is returned;   
            = 'I':  U is initialized to the unit matrix, and the   
                    orthogonal matrix U is returned;   
            = 'N':  U is not computed.   

    JOBV    (input) CHARACTER*1   
            = 'V':  V must contain an orthogonal matrix V1 on entry, and   
                    the product V1*V is returned;   
            = 'I':  V is initialized to the unit matrix, and the   
                    orthogonal matrix V is returned;   
            = 'N':  V is not computed.   

    JOBQ    (input) CHARACTER*1   
            = 'Q':  Q must contain an orthogonal matrix Q1 on entry, and   
                    the product Q1*Q is returned;   
            = 'I':  Q is initialized to the unit matrix, and the   
                    orthogonal matrix Q is returned;   
            = 'N':  Q is not computed.   

    M       (input) INTEGER   
            The number of rows of the matrix A.  M >= 0.   

    P       (input) INTEGER   
            The number of rows of the matrix B.  P >= 0.   

    N       (input) INTEGER   
            The number of columns of the matrices A and B.  N >= 0.   

    K       (input) INTEGER   
    L       (input) INTEGER   
            K and L specify the subblocks in the input matrices A and B:   
            A23 = A(K+1:MIN(K+L,M),N-L+1:N) and B13 = B(1:L,N-L+1:N)   
            of A and B, whose GSVD is going to be computed by DTGSJA.   
            See Further details.   

    A       (input/output) DOUBLE PRECISION array, dimension (LDA,N)   
            On entry, the M-by-N matrix A.   
            On exit, A(N-K+1:N,1:MIN(K+L,M) ) contains the triangular   
            matrix R or part of R.  See Purpose for details.   

    LDA     (input) INTEGER   
            The leading dimension of the array A. LDA >= max(1,M).   

    B       (input/output) DOUBLE PRECISION array, dimension (LDB,N)   
            On entry, the P-by-N matrix B.   
            On exit, if necessary, B(M-K+1:L,N+M-K-L+1:N) contains   
            a part of R.  See Purpose for details.   

    LDB     (input) INTEGER   
            The leading dimension of the array B. LDB >= max(1,P).   

    TOLA    (input) DOUBLE PRECISION   
    TOLB    (input) DOUBLE PRECISION   
            TOLA and TOLB are the convergence criteria for the Jacobi-   
            Kogbetliantz iteration procedure. Generally, they are the   
            same as used in the preprocessing step, say   
                TOLA = max(M,N)*norm(A)*MAZHEPS,   
                TOLB = max(P,N)*norm(B)*MAZHEPS.   

    ALPHA   (output) DOUBLE PRECISION array, dimension (N)   
    BETA    (output) DOUBLE PRECISION array, dimension (N)   
            On exit, ALPHA and BETA contain the generalized singular   
            value pairs of A and B;   
              ALPHA(1:K) = 1,   
              BETA(1:K)  = 0,   
            and if M-K-L >= 0,   
              ALPHA(K+1:K+L) = diag(C),   
              BETA(K+1:K+L)  = diag(S),   
            or if M-K-L < 0,   
              ALPHA(K+1:M)= C, ALPHA(M+1:K+L)= 0   
              BETA(K+1:M) = S, BETA(M+1:K+L) = 1.   
            Furthermore, if K+L < N,   
              ALPHA(K+L+1:N) = 0 and   
              BETA(K+L+1:N)  = 0.   

    U       (input/output) DOUBLE PRECISION array, dimension (LDU,M)   
            On entry, if JOBU = 'U', U must contain a matrix U1 (usually   
            the orthogonal matrix returned by DGGSVP).   
            On exit,   
            if JOBU = 'I', U contains the orthogonal matrix U;   
            if JOBU = 'U', U contains the product U1*U.   
            If JOBU = 'N', U is not referenced.   

    LDU     (input) INTEGER   
            The leading dimension of the array U. LDU >= max(1,M) if   
            JOBU = 'U'; LDU >= 1 otherwise.   

    V       (input/output) DOUBLE PRECISION array, dimension (LDV,P)   
            On entry, if JOBV = 'V', V must contain a matrix V1 (usually   
            the orthogonal matrix returned by DGGSVP).   
            On exit,   
            if JOBV = 'I', V contains the orthogonal matrix V;   
            if JOBV = 'V', V contains the product V1*V.   
            If JOBV = 'N', V is not referenced.   

    LDV     (input) INTEGER   
            The leading dimension of the array V. LDV >= max(1,P) if   
            JOBV = 'V'; LDV >= 1 otherwise.   

    Q       (input/output) DOUBLE PRECISION array, dimension (LDQ,N)   
            On entry, if JOBQ = 'Q', Q must contain a matrix Q1 (usually   
            the orthogonal matrix returned by DGGSVP).   
            On exit,   
            if JOBQ = 'I', Q contains the orthogonal matrix Q;   
            if JOBQ = 'Q', Q contains the product Q1*Q.   
            If JOBQ = 'N', Q is not referenced.   

    LDQ     (input) INTEGER   
            The leading dimension of the array Q. LDQ >= max(1,N) if   
            JOBQ = 'Q'; LDQ >= 1 otherwise.   

    WORK    (workspace) DOUBLE PRECISION array, dimension (2*N)   

    NCYCLE  (output) INTEGER   
            The number of cycles required for convergence.   

    INFO    (output) INTEGER   
            = 0:  successful exit   
            < 0:  if INFO = -i, the i-th argument had an illegal value.   
            = 1:  the procedure does not converge after MAXIT cycles.   

    Internal Parameters   
    ===================   

    MAXIT   INTEGER   
            MAXIT specifies the total loops that the iterative procedure   
            may take. If after MAXIT cycles, the routine fails to   
            converge, we return INFO = 1.   

    Further Details   
    ===============   

    DTGSJA essentially uses a variant of Kogbetliantz algorithm to reduce   
    min(L,M-K)-by-L triangular (or trapezoidal) matrix A23 and L-by-L   
    matrix B13 to the form:   

             U1'*A13*Q1 = C1*R1; V1'*B13*Q1 = S1*R1,   

    where U1, V1 and Q1 are orthogonal matrix, and Z' is the transpose   
    of Z.  C1 and S1 are diagonal matrices satisfying   

                  C1**2 + S1**2 = I,   

    and R1 is an L-by-L nonsingular upper triangular matrix.   

    =====================================================================   



       Decode and test the input parameters   

       Parameter adjustments */
    /* Table of constant values */
    static doublereal c_b13 = 0.;
    static doublereal c_b14 = 1.;
    static integer c__1 = 1;
    static doublereal c_b43 = -1.;
    
    /* System generated locals */
    integer a_dim1, a_offset, b_dim1, b_offset, q_dim1, q_offset, u_dim1, 
	    u_offset, v_dim1, v_offset, i__1, i__2, i__3, i__4;
    doublereal d__1;
    /* Local variables */
    extern /* Subroutine */ int drot_(integer *, doublereal *, integer *, 
	    doublereal *, integer *, doublereal *, doublereal *);
    static integer i__, j;
    static doublereal gamma;
    extern /* Subroutine */ int dscal_(integer *, doublereal *, doublereal *, 
	    integer *);
    extern logical lsame_(char *, char *);
    extern /* Subroutine */ int dcopy_(integer *, doublereal *, integer *, 
	    doublereal *, integer *);
    static doublereal a1;
    static logical initq;
    static doublereal a2, a3, b1;
    static logical initu, initv, wantq, upper;
    static doublereal b2, b3;
    static logical wantu, wantv;
    static doublereal error, ssmin;
    extern /* Subroutine */ int dlags2_(logical *, doublereal *, doublereal *,
	     doublereal *, doublereal *, doublereal *, doublereal *, 
	    doublereal *, doublereal *, doublereal *, doublereal *, 
	    doublereal *, doublereal *), dlapll_(integer *, doublereal *, 
	    integer *, doublereal *, integer *, doublereal *);
    static integer kcycle;
    extern /* Subroutine */ int dlartg_(doublereal *, doublereal *, 
	    doublereal *, doublereal *, doublereal *), dlaset_(char *, 
	    integer *, integer *, doublereal *, doublereal *, doublereal *, 
	    integer *), xerbla_(char *, integer *);
    static doublereal csq, csu, csv, snq, rwk, snu, snv;
#define a_ref(a_1,a_2) a[(a_2)*a_dim1 + a_1]
#define b_ref(a_1,a_2) b[(a_2)*b_dim1 + a_1]
#define q_ref(a_1,a_2) q[(a_2)*q_dim1 + a_1]
#define u_ref(a_1,a_2) u[(a_2)*u_dim1 + a_1]
#define v_ref(a_1,a_2) v[(a_2)*v_dim1 + a_1]


    a_dim1 = *lda;
    a_offset = 1 + a_dim1 * 1;
    a -= a_offset;
    b_dim1 = *ldb;
    b_offset = 1 + b_dim1 * 1;
    b -= b_offset;
    --alpha;
    --beta;
    u_dim1 = *ldu;
    u_offset = 1 + u_dim1 * 1;
    u -= u_offset;
    v_dim1 = *ldv;
    v_offset = 1 + v_dim1 * 1;
    v -= v_offset;
    q_dim1 = *ldq;
    q_offset = 1 + q_dim1 * 1;
    q -= q_offset;
    --work;

    /* Function Body */
    initu = lsame_(jobu, "I");
    wantu = initu || lsame_(jobu, "U");

    initv = lsame_(jobv, "I");
    wantv = initv || lsame_(jobv, "V");

    initq = lsame_(jobq, "I");
    wantq = initq || lsame_(jobq, "Q");

    *info = 0;
    if (! (initu || wantu || lsame_(jobu, "N"))) {
	*info = -1;
    } else if (! (initv || wantv || lsame_(jobv, "N"))) 
	    {
	*info = -2;
    } else if (! (initq || wantq || lsame_(jobq, "N"))) 
	    {
	*info = -3;
    } else if (*m < 0) {
	*info = -4;
    } else if (*p < 0) {
	*info = -5;
    } else if (*n < 0) {
	*info = -6;
    } else if (*lda < max(1,*m)) {
	*info = -10;
    } else if (*ldb < max(1,*p)) {
	*info = -12;
    } else if (*ldu < 1 || wantu && *ldu < *m) {
	*info = -18;
    } else if (*ldv < 1 || wantv && *ldv < *p) {
	*info = -20;
    } else if (*ldq < 1 || wantq && *ldq < *n) {
	*info = -22;
    }
    if (*info != 0) {
	i__1 = -(*info);
	xerbla_("DTGSJA", &i__1);
	return 0;
    }

/*     Initialize U, V and Q, if necessary */

    if (initu) {
	dlaset_("Full", m, m, &c_b13, &c_b14, &u[u_offset], ldu);
    }
    if (initv) {
	dlaset_("Full", p, p, &c_b13, &c_b14, &v[v_offset], ldv);
    }
    if (initq) {
	dlaset_("Full", n, n, &c_b13, &c_b14, &q[q_offset], ldq);
    }

/*     Loop until convergence */

    upper = FALSE_;
    for (kcycle = 1; kcycle <= 40; ++kcycle) {

	upper = ! upper;

	i__1 = *l - 1;
	for (i__ = 1; i__ <= i__1; ++i__) {
	    i__2 = *l;
	    for (j = i__ + 1; j <= i__2; ++j) {

		a1 = 0.;
		a2 = 0.;
		a3 = 0.;
		if (*k + i__ <= *m) {
		    a1 = a_ref(*k + i__, *n - *l + i__);
		}
		if (*k + j <= *m) {
		    a3 = a_ref(*k + j, *n - *l + j);
		}

		b1 = b_ref(i__, *n - *l + i__);
		b3 = b_ref(j, *n - *l + j);

		if (upper) {
		    if (*k + i__ <= *m) {
			a2 = a_ref(*k + i__, *n - *l + j);
		    }
		    b2 = b_ref(i__, *n - *l + j);
		} else {
		    if (*k + j <= *m) {
			a2 = a_ref(*k + j, *n - *l + i__);
		    }
		    b2 = b_ref(j, *n - *l + i__);
		}

		dlags2_(&upper, &a1, &a2, &a3, &b1, &b2, &b3, &csu, &snu, &
			csv, &snv, &csq, &snq);

/*              Update (K+I)-th and (K+J)-th rows of matrix A: U'*A */

		if (*k + j <= *m) {
		    drot_(l, &a_ref(*k + j, *n - *l + 1), lda, &a_ref(*k + 
			    i__, *n - *l + 1), lda, &csu, &snu);
		}

/*              Update I-th and J-th rows of matrix B: V'*B */

		drot_(l, &b_ref(j, *n - *l + 1), ldb, &b_ref(i__, *n - *l + 1)
			, ldb, &csv, &snv);

/*              Update (N-L+I)-th and (N-L+J)-th columns of matrices   
                A and B: A*Q and B*Q   

   Computing MIN */
		i__4 = *k + *l;
		i__3 = min(i__4,*m);
		drot_(&i__3, &a_ref(1, *n - *l + j), &c__1, &a_ref(1, *n - *l 
			+ i__), &c__1, &csq, &snq);

		drot_(l, &b_ref(1, *n - *l + j), &c__1, &b_ref(1, *n - *l + 
			i__), &c__1, &csq, &snq);

		if (upper) {
		    if (*k + i__ <= *m) {
			a_ref(*k + i__, *n - *l + j) = 0.;
		    }
		    b_ref(i__, *n - *l + j) = 0.;
		} else {
		    if (*k + j <= *m) {
			a_ref(*k + j, *n - *l + i__) = 0.;
		    }
		    b_ref(j, *n - *l + i__) = 0.;
		}

/*              Update orthogonal matrices U, V, Q, if desired. */

		if (wantu && *k + j <= *m) {
		    drot_(m, &u_ref(1, *k + j), &c__1, &u_ref(1, *k + i__), &
			    c__1, &csu, &snu);
		}

		if (wantv) {
		    drot_(p, &v_ref(1, j), &c__1, &v_ref(1, i__), &c__1, &csv,
			     &snv);
		}

		if (wantq) {
		    drot_(n, &q_ref(1, *n - *l + j), &c__1, &q_ref(1, *n - *l 
			    + i__), &c__1, &csq, &snq);
		}

/* L10: */
	    }
/* L20: */
	}

	if (! upper) {

/*           The matrices A13 and B13 were lower triangular at the start   
             of the cycle, and are now upper triangular.   

             Convergence test: test the parallelism of the corresponding   
             rows of A and B. */

	    error = 0.;
/* Computing MIN */
	    i__2 = *l, i__3 = *m - *k;
	    i__1 = min(i__2,i__3);
	    for (i__ = 1; i__ <= i__1; ++i__) {
		i__2 = *l - i__ + 1;
		dcopy_(&i__2, &a_ref(*k + i__, *n - *l + i__), lda, &work[1], 
			&c__1);
		i__2 = *l - i__ + 1;
		dcopy_(&i__2, &b_ref(i__, *n - *l + i__), ldb, &work[*l + 1], 
			&c__1);
		i__2 = *l - i__ + 1;
		dlapll_(&i__2, &work[1], &c__1, &work[*l + 1], &c__1, &ssmin);
		error = max(error,ssmin);
/* L30: */
	    }

	    if (abs(error) <= min(*tola,*tolb)) {
		goto L50;
	    }
	}

/*        End of cycle loop   

   L40: */
    }

/*     The algorithm has not converged after MAXIT cycles. */

    *info = 1;
    goto L100;

L50:

/*     If ERROR <= MIN(TOLA,TOLB), then the algorithm has converged.   
       Compute the generalized singular value pairs (ALPHA, BETA), and   
       set the triangular matrix R to array A. */

    i__1 = *k;
    for (i__ = 1; i__ <= i__1; ++i__) {
	alpha[i__] = 1.;
	beta[i__] = 0.;
/* L60: */
    }

/* Computing MIN */
    i__2 = *l, i__3 = *m - *k;
    i__1 = min(i__2,i__3);
    for (i__ = 1; i__ <= i__1; ++i__) {

	a1 = a_ref(*k + i__, *n - *l + i__);
	b1 = b_ref(i__, *n - *l + i__);

	if (a1 != 0.) {
	    gamma = b1 / a1;

/*           change sign if necessary */

	    if (gamma < 0.) {
		i__2 = *l - i__ + 1;
		dscal_(&i__2, &c_b43, &b_ref(i__, *n - *l + i__), ldb);
		if (wantv) {
		    dscal_(p, &c_b43, &v_ref(1, i__), &c__1);
		}
	    }

	    d__1 = abs(gamma);
	    dlartg_(&d__1, &c_b14, &beta[*k + i__], &alpha[*k + i__], &rwk);

	    if (alpha[*k + i__] >= beta[*k + i__]) {
		i__2 = *l - i__ + 1;
		d__1 = 1. / alpha[*k + i__];
		dscal_(&i__2, &d__1, &a_ref(*k + i__, *n - *l + i__), lda);
	    } else {
		i__2 = *l - i__ + 1;
		d__1 = 1. / beta[*k + i__];
		dscal_(&i__2, &d__1, &b_ref(i__, *n - *l + i__), ldb);
		i__2 = *l - i__ + 1;
		dcopy_(&i__2, &b_ref(i__, *n - *l + i__), ldb, &a_ref(*k + 
			i__, *n - *l + i__), lda);
	    }

	} else {

	    alpha[*k + i__] = 0.;
	    beta[*k + i__] = 1.;
	    i__2 = *l - i__ + 1;
	    dcopy_(&i__2, &b_ref(i__, *n - *l + i__), ldb, &a_ref(*k + i__, *
		    n - *l + i__), lda);

	}

/* L70: */
    }

/*     Post-assignment */

    i__1 = *k + *l;
    for (i__ = *m + 1; i__ <= i__1; ++i__) {
	alpha[i__] = 0.;
	beta[i__] = 1.;
/* L80: */
    }

    if (*k + *l < *n) {
	i__1 = *n;
	for (i__ = *k + *l + 1; i__ <= i__1; ++i__) {
	    alpha[i__] = 0.;
	    beta[i__] = 0.;
/* L90: */
	}
    }

L100:
    *ncycle = kcycle;
    return 0;

/*     End of DTGSJA */

} /* dtgsja_ */
Beispiel #9
0
VectorXd velocityReference(NewQPControllerData *pdata, double t, const Ref<VectorXd> &q, const Ref<VectorXd> &qd, const Ref<VectorXd> &qdd, bool foot_contact[2], VRefIntegratorParams *params, RobotPropertyCache *rpc) {
  // Integrate expected accelerations to determine a target feed-forward velocity, which we can pass in to Atlas
  int i;
  assert(qdd.size() == pdata->r->num_velocities);

  double dt = 0;
  if (pdata->state.t_prev != 0) {
    dt = t - pdata->state.t_prev;
  }

  pdata->state.vref_integrator_state = (1-params->eta)*pdata->state.vref_integrator_state + params->eta*qd + qdd*dt;

  if (params->zero_ankles_on_contact && foot_contact[0] == 1) {
    for (i=0; i < rpc->position_indices.l_leg_ak.size(); i++) {
      pdata->state.vref_integrator_state(rpc->position_indices.l_leg_ak(i)) = 0;
    }
  }
  if (params->zero_ankles_on_contact && foot_contact[1] == 1) {
    for (i=0; i < rpc->position_indices.r_leg_ak.size(); i++) {
      pdata->state.vref_integrator_state(rpc->position_indices.r_leg_ak(i)) = 0;
    }
  }
  if (pdata->state.foot_contact_prev[0] != foot_contact[0]) {
    // contact state changed, reset integrated velocities
    for (i=0; i < rpc->position_indices.l_leg.size(); i++) {
      pdata->state.vref_integrator_state(rpc->position_indices.l_leg(i)) = qd(rpc->position_indices.l_leg(i));
    }
  }
  if (pdata->state.foot_contact_prev[1] != foot_contact[1]) {
    // contact state changed, reset integrated velocities
    for (i=0; i < rpc->position_indices.r_leg.size(); i++) {
      pdata->state.vref_integrator_state(rpc->position_indices.r_leg(i)) = qd(rpc->position_indices.r_leg(i));
    }
  }

  pdata->state.foot_contact_prev[0] = foot_contact[0];
  pdata->state.foot_contact_prev[1] = foot_contact[1];

  VectorXd qd_err = pdata->state.vref_integrator_state - qd;

  // do not velocity control ankles when in contact
  if (params->zero_ankles_on_contact && foot_contact[0] == 1) {
    for (i=0; i < rpc->position_indices.l_leg_ak.size(); i++) {
      qd_err(rpc->position_indices.l_leg_ak(i)) = 0;
    }
  }
  if (params->zero_ankles_on_contact && foot_contact[1] == 1) {
    for (i=0; i < rpc->position_indices.r_leg_ak.size(); i++) {
      qd_err(rpc->position_indices.r_leg_ak(i)) = 0;
    }
  }

  double delta_max = 1.0;
  VectorXd v_ref = VectorXd::Zero(rpc->actuated_indices.size());
  for (i=0; i < rpc->actuated_indices.size(); i++) {
    v_ref(i) = qd_err(rpc->actuated_indices(i));
  }
  v_ref = v_ref.array().max(-delta_max);
  v_ref = v_ref.array().min(delta_max);
  return v_ref;
}
Beispiel #10
0
/* Subroutine */ int dlaqtr_(logical *ltran, logical *lreal, integer *n, 
	doublereal *t, integer *ldt, doublereal *b, doublereal *w, doublereal 
	*scale, doublereal *x, doublereal *work, integer *info)
{
/*  -- LAPACK auxiliary routine (version 3.0) --   
       Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,   
       Courant Institute, Argonne National Lab, and Rice University   
       June 30, 1999   


    Purpose   
    =======   

    DLAQTR solves the real quasi-triangular system   

                 op(T)*p = scale*c,               if LREAL = .TRUE.   

    or the complex quasi-triangular systems   

               op(T + iB)*(p+iq) = scale*(c+id),  if LREAL = .FALSE.   

    in real arithmetic, where T is upper quasi-triangular.   
    If LREAL = .FALSE., then the first diagonal block of T must be   
    1 by 1, B is the specially structured matrix   

                   B = [ b(1) b(2) ... b(n) ]   
                       [       w            ]   
                       [           w        ]   
                       [              .     ]   
                       [                 w  ]   

    op(A) = A or A', A' denotes the conjugate transpose of   
    matrix A.   

    On input, X = [ c ].  On output, X = [ p ].   
                  [ d ]                  [ q ]   

    This subroutine is designed for the condition number estimation   
    in routine DTRSNA.   

    Arguments   
    =========   

    LTRAN   (input) LOGICAL   
            On entry, LTRAN specifies the option of conjugate transpose:   
               = .FALSE.,    op(T+i*B) = T+i*B,   
               = .TRUE.,     op(T+i*B) = (T+i*B)'.   

    LREAL   (input) LOGICAL   
            On entry, LREAL specifies the input matrix structure:   
               = .FALSE.,    the input is complex   
               = .TRUE.,     the input is real   

    N       (input) INTEGER   
            On entry, N specifies the order of T+i*B. N >= 0.   

    T       (input) DOUBLE PRECISION array, dimension (LDT,N)   
            On entry, T contains a matrix in Schur canonical form.   
            If LREAL = .FALSE., then the first diagonal block of T mu   
            be 1 by 1.   

    LDT     (input) INTEGER   
            The leading dimension of the matrix T. LDT >= max(1,N).   

    B       (input) DOUBLE PRECISION array, dimension (N)   
            On entry, B contains the elements to form the matrix   
            B as described above.   
            If LREAL = .TRUE., B is not referenced.   

    W       (input) DOUBLE PRECISION   
            On entry, W is the diagonal element of the matrix B.   
            If LREAL = .TRUE., W is not referenced.   

    SCALE   (output) DOUBLE PRECISION   
            On exit, SCALE is the scale factor.   

    X       (input/output) DOUBLE PRECISION array, dimension (2*N)   
            On entry, X contains the right hand side of the system.   
            On exit, X is overwritten by the solution.   

    WORK    (workspace) DOUBLE PRECISION array, dimension (N)   

    INFO    (output) INTEGER   
            On exit, INFO is set to   
               0: successful exit.   
                 1: the some diagonal 1 by 1 block has been perturbed by   
                    a small number SMIN to keep nonsingularity.   
                 2: the some diagonal 2 by 2 block has been perturbed by   
                    a small number in DLALN2 to keep nonsingularity.   
            NOTE: In the interests of speed, this routine does not   
                  check the inputs for errors.   

   =====================================================================   


       Do not test the input parameters for errors   

       Parameter adjustments */
    /* Table of constant values */
    static integer c__1 = 1;
    static logical c_false = FALSE_;
    static integer c__2 = 2;
    static doublereal c_b21 = 1.;
    static doublereal c_b25 = 0.;
    static logical c_true = TRUE_;
    
    /* System generated locals */
    integer t_dim1, t_offset, i__1, i__2;
    doublereal d__1, d__2, d__3, d__4, d__5, d__6;
    /* Local variables */
    extern doublereal ddot_(integer *, doublereal *, integer *, doublereal *, 
	    integer *);
    static integer ierr;
    static doublereal smin, xmax, d__[4]	/* was [2][2] */;
    static integer i__, j, k;
    static doublereal v[4]	/* was [2][2] */, z__;
    extern /* Subroutine */ int dscal_(integer *, doublereal *, doublereal *, 
	    integer *);
    extern doublereal dasum_(integer *, doublereal *, integer *);
    extern /* Subroutine */ int daxpy_(integer *, doublereal *, doublereal *, 
	    integer *, doublereal *, integer *);
    static integer jnext, j1, j2;
    static doublereal sminw;
    static integer n1, n2;
    static doublereal xnorm;
    extern /* Subroutine */ int dlaln2_(logical *, integer *, integer *, 
	    doublereal *, doublereal *, doublereal *, integer *, doublereal *,
	     doublereal *, doublereal *, integer *, doublereal *, doublereal *
	    , doublereal *, integer *, doublereal *, doublereal *, integer *);
    extern doublereal dlamch_(char *), dlange_(char *, integer *, 
	    integer *, doublereal *, integer *, doublereal *);
    static doublereal si, xj;
    extern integer idamax_(integer *, doublereal *, integer *);
    static doublereal scaloc, sr;
    extern /* Subroutine */ int dladiv_(doublereal *, doublereal *, 
	    doublereal *, doublereal *, doublereal *, doublereal *);
    static doublereal bignum;
    static logical notran;
    static doublereal smlnum, rec, eps, tjj, tmp;
#define d___ref(a_1,a_2) d__[(a_2)*2 + a_1 - 3]
#define t_ref(a_1,a_2) t[(a_2)*t_dim1 + a_1]
#define v_ref(a_1,a_2) v[(a_2)*2 + a_1 - 3]


    t_dim1 = *ldt;
    t_offset = 1 + t_dim1 * 1;
    t -= t_offset;
    --b;
    --x;
    --work;

    /* Function Body */
    notran = ! (*ltran);
    *info = 0;

/*     Quick return if possible */

    if (*n == 0) {
	return 0;
    }

/*     Set constants to control overflow */

    eps = dlamch_("P");
    smlnum = dlamch_("S") / eps;
    bignum = 1. / smlnum;

    xnorm = dlange_("M", n, n, &t[t_offset], ldt, d__);
    if (! (*lreal)) {
/* Computing MAX */
	d__1 = xnorm, d__2 = abs(*w), d__1 = max(d__1,d__2), d__2 = dlange_(
		"M", n, &c__1, &b[1], n, d__);
	xnorm = max(d__1,d__2);
    }
/* Computing MAX */
    d__1 = smlnum, d__2 = eps * xnorm;
    smin = max(d__1,d__2);

/*     Compute 1-norm of each column of strictly upper triangular   
       part of T to control overflow in triangular solver. */

    work[1] = 0.;
    i__1 = *n;
    for (j = 2; j <= i__1; ++j) {
	i__2 = j - 1;
	work[j] = dasum_(&i__2, &t_ref(1, j), &c__1);
/* L10: */
    }

    if (! (*lreal)) {
	i__1 = *n;
	for (i__ = 2; i__ <= i__1; ++i__) {
	    work[i__] += (d__1 = b[i__], abs(d__1));
/* L20: */
	}
    }

    n2 = *n << 1;
    n1 = *n;
    if (! (*lreal)) {
	n1 = n2;
    }
    k = idamax_(&n1, &x[1], &c__1);
    xmax = (d__1 = x[k], abs(d__1));
    *scale = 1.;

    if (xmax > bignum) {
	*scale = bignum / xmax;
	dscal_(&n1, scale, &x[1], &c__1);
	xmax = bignum;
    }

    if (*lreal) {

	if (notran) {

/*           Solve T*p = scale*c */

	    jnext = *n;
	    for (j = *n; j >= 1; --j) {
		if (j > jnext) {
		    goto L30;
		}
		j1 = j;
		j2 = j;
		jnext = j - 1;
		if (j > 1) {
		    if (t_ref(j, j - 1) != 0.) {
			j1 = j - 1;
			jnext = j - 2;
		    }
		}

		if (j1 == j2) {

/*                 Meet 1 by 1 diagonal block   

                   Scale to avoid overflow when computing   
                       x(j) = b(j)/T(j,j) */

		    xj = (d__1 = x[j1], abs(d__1));
		    tjj = (d__1 = t_ref(j1, j1), abs(d__1));
		    tmp = t_ref(j1, j1);
		    if (tjj < smin) {
			tmp = smin;
			tjj = smin;
			*info = 1;
		    }

		    if (xj == 0.) {
			goto L30;
		    }

		    if (tjj < 1.) {
			if (xj > bignum * tjj) {
			    rec = 1. / xj;
			    dscal_(n, &rec, &x[1], &c__1);
			    *scale *= rec;
			    xmax *= rec;
			}
		    }
		    x[j1] /= tmp;
		    xj = (d__1 = x[j1], abs(d__1));

/*                 Scale x if necessary to avoid overflow when adding a   
                   multiple of column j1 of T. */

		    if (xj > 1.) {
			rec = 1. / xj;
			if (work[j1] > (bignum - xmax) * rec) {
			    dscal_(n, &rec, &x[1], &c__1);
			    *scale *= rec;
			}
		    }
		    if (j1 > 1) {
			i__1 = j1 - 1;
			d__1 = -x[j1];
			daxpy_(&i__1, &d__1, &t_ref(1, j1), &c__1, &x[1], &
				c__1);
			i__1 = j1 - 1;
			k = idamax_(&i__1, &x[1], &c__1);
			xmax = (d__1 = x[k], abs(d__1));
		    }

		} else {

/*                 Meet 2 by 2 diagonal block   

                   Call 2 by 2 linear system solve, to take   
                   care of possible overflow by scaling factor. */

		    d___ref(1, 1) = x[j1];
		    d___ref(2, 1) = x[j2];
		    dlaln2_(&c_false, &c__2, &c__1, &smin, &c_b21, &t_ref(j1, 
			    j1), ldt, &c_b21, &c_b21, d__, &c__2, &c_b25, &
			    c_b25, v, &c__2, &scaloc, &xnorm, &ierr);
		    if (ierr != 0) {
			*info = 2;
		    }

		    if (scaloc != 1.) {
			dscal_(n, &scaloc, &x[1], &c__1);
			*scale *= scaloc;
		    }
		    x[j1] = v_ref(1, 1);
		    x[j2] = v_ref(2, 1);

/*                 Scale V(1,1) (= X(J1)) and/or V(2,1) (=X(J2))   
                   to avoid overflow in updating right-hand side.   

   Computing MAX */
		    d__3 = (d__1 = v_ref(1, 1), abs(d__1)), d__4 = (d__2 = 
			    v_ref(2, 1), abs(d__2));
		    xj = max(d__3,d__4);
		    if (xj > 1.) {
			rec = 1. / xj;
/* Computing MAX */
			d__1 = work[j1], d__2 = work[j2];
			if (max(d__1,d__2) > (bignum - xmax) * rec) {
			    dscal_(n, &rec, &x[1], &c__1);
			    *scale *= rec;
			}
		    }

/*                 Update right-hand side */

		    if (j1 > 1) {
			i__1 = j1 - 1;
			d__1 = -x[j1];
			daxpy_(&i__1, &d__1, &t_ref(1, j1), &c__1, &x[1], &
				c__1);
			i__1 = j1 - 1;
			d__1 = -x[j2];
			daxpy_(&i__1, &d__1, &t_ref(1, j2), &c__1, &x[1], &
				c__1);
			i__1 = j1 - 1;
			k = idamax_(&i__1, &x[1], &c__1);
			xmax = (d__1 = x[k], abs(d__1));
		    }

		}

L30:
		;
	    }

	} else {

/*           Solve T'*p = scale*c */

	    jnext = 1;
	    i__1 = *n;
	    for (j = 1; j <= i__1; ++j) {
		if (j < jnext) {
		    goto L40;
		}
		j1 = j;
		j2 = j;
		jnext = j + 1;
		if (j < *n) {
		    if (t_ref(j + 1, j) != 0.) {
			j2 = j + 1;
			jnext = j + 2;
		    }
		}

		if (j1 == j2) {

/*                 1 by 1 diagonal block   

                   Scale if necessary to avoid overflow in forming the   
                   right-hand side element by inner product. */

		    xj = (d__1 = x[j1], abs(d__1));
		    if (xmax > 1.) {
			rec = 1. / xmax;
			if (work[j1] > (bignum - xj) * rec) {
			    dscal_(n, &rec, &x[1], &c__1);
			    *scale *= rec;
			    xmax *= rec;
			}
		    }

		    i__2 = j1 - 1;
		    x[j1] -= ddot_(&i__2, &t_ref(1, j1), &c__1, &x[1], &c__1);

		    xj = (d__1 = x[j1], abs(d__1));
		    tjj = (d__1 = t_ref(j1, j1), abs(d__1));
		    tmp = t_ref(j1, j1);
		    if (tjj < smin) {
			tmp = smin;
			tjj = smin;
			*info = 1;
		    }

		    if (tjj < 1.) {
			if (xj > bignum * tjj) {
			    rec = 1. / xj;
			    dscal_(n, &rec, &x[1], &c__1);
			    *scale *= rec;
			    xmax *= rec;
			}
		    }
		    x[j1] /= tmp;
/* Computing MAX */
		    d__2 = xmax, d__3 = (d__1 = x[j1], abs(d__1));
		    xmax = max(d__2,d__3);

		} else {

/*                 2 by 2 diagonal block   

                   Scale if necessary to avoid overflow in forming the   
                   right-hand side elements by inner product.   

   Computing MAX */
		    d__3 = (d__1 = x[j1], abs(d__1)), d__4 = (d__2 = x[j2], 
			    abs(d__2));
		    xj = max(d__3,d__4);
		    if (xmax > 1.) {
			rec = 1. / xmax;
/* Computing MAX */
			d__1 = work[j2], d__2 = work[j1];
			if (max(d__1,d__2) > (bignum - xj) * rec) {
			    dscal_(n, &rec, &x[1], &c__1);
			    *scale *= rec;
			    xmax *= rec;
			}
		    }

		    i__2 = j1 - 1;
		    d___ref(1, 1) = x[j1] - ddot_(&i__2, &t_ref(1, j1), &c__1,
			     &x[1], &c__1);
		    i__2 = j1 - 1;
		    d___ref(2, 1) = x[j2] - ddot_(&i__2, &t_ref(1, j2), &c__1,
			     &x[1], &c__1);

		    dlaln2_(&c_true, &c__2, &c__1, &smin, &c_b21, &t_ref(j1, 
			    j1), ldt, &c_b21, &c_b21, d__, &c__2, &c_b25, &
			    c_b25, v, &c__2, &scaloc, &xnorm, &ierr);
		    if (ierr != 0) {
			*info = 2;
		    }

		    if (scaloc != 1.) {
			dscal_(n, &scaloc, &x[1], &c__1);
			*scale *= scaloc;
		    }
		    x[j1] = v_ref(1, 1);
		    x[j2] = v_ref(2, 1);
/* Computing MAX */
		    d__3 = (d__1 = x[j1], abs(d__1)), d__4 = (d__2 = x[j2], 
			    abs(d__2)), d__3 = max(d__3,d__4);
		    xmax = max(d__3,xmax);

		}
L40:
		;
	    }
	}

    } else {

/* Computing MAX */
	d__1 = eps * abs(*w);
	sminw = max(d__1,smin);
	if (notran) {

/*           Solve (T + iB)*(p+iq) = c+id */

	    jnext = *n;
	    for (j = *n; j >= 1; --j) {
		if (j > jnext) {
		    goto L70;
		}
		j1 = j;
		j2 = j;
		jnext = j - 1;
		if (j > 1) {
		    if (t_ref(j, j - 1) != 0.) {
			j1 = j - 1;
			jnext = j - 2;
		    }
		}

		if (j1 == j2) {

/*                 1 by 1 diagonal block   

                   Scale if necessary to avoid overflow in division */

		    z__ = *w;
		    if (j1 == 1) {
			z__ = b[1];
		    }
		    xj = (d__1 = x[j1], abs(d__1)) + (d__2 = x[*n + j1], abs(
			    d__2));
		    tjj = (d__1 = t_ref(j1, j1), abs(d__1)) + abs(z__);
		    tmp = t_ref(j1, j1);
		    if (tjj < sminw) {
			tmp = sminw;
			tjj = sminw;
			*info = 1;
		    }

		    if (xj == 0.) {
			goto L70;
		    }

		    if (tjj < 1.) {
			if (xj > bignum * tjj) {
			    rec = 1. / xj;
			    dscal_(&n2, &rec, &x[1], &c__1);
			    *scale *= rec;
			    xmax *= rec;
			}
		    }
		    dladiv_(&x[j1], &x[*n + j1], &tmp, &z__, &sr, &si);
		    x[j1] = sr;
		    x[*n + j1] = si;
		    xj = (d__1 = x[j1], abs(d__1)) + (d__2 = x[*n + j1], abs(
			    d__2));

/*                 Scale x if necessary to avoid overflow when adding a   
                   multiple of column j1 of T. */

		    if (xj > 1.) {
			rec = 1. / xj;
			if (work[j1] > (bignum - xmax) * rec) {
			    dscal_(&n2, &rec, &x[1], &c__1);
			    *scale *= rec;
			}
		    }

		    if (j1 > 1) {
			i__1 = j1 - 1;
			d__1 = -x[j1];
			daxpy_(&i__1, &d__1, &t_ref(1, j1), &c__1, &x[1], &
				c__1);
			i__1 = j1 - 1;
			d__1 = -x[*n + j1];
			daxpy_(&i__1, &d__1, &t_ref(1, j1), &c__1, &x[*n + 1],
				 &c__1);

			x[1] += b[j1] * x[*n + j1];
			x[*n + 1] -= b[j1] * x[j1];

			xmax = 0.;
			i__1 = j1 - 1;
			for (k = 1; k <= i__1; ++k) {
/* Computing MAX */
			    d__3 = xmax, d__4 = (d__1 = x[k], abs(d__1)) + (
				    d__2 = x[k + *n], abs(d__2));
			    xmax = max(d__3,d__4);
/* L50: */
			}
		    }

		} else {

/*                 Meet 2 by 2 diagonal block */

		    d___ref(1, 1) = x[j1];
		    d___ref(2, 1) = x[j2];
		    d___ref(1, 2) = x[*n + j1];
		    d___ref(2, 2) = x[*n + j2];
		    d__1 = -(*w);
		    dlaln2_(&c_false, &c__2, &c__2, &sminw, &c_b21, &t_ref(j1,
			     j1), ldt, &c_b21, &c_b21, d__, &c__2, &c_b25, &
			    d__1, v, &c__2, &scaloc, &xnorm, &ierr);
		    if (ierr != 0) {
			*info = 2;
		    }

		    if (scaloc != 1.) {
			i__1 = *n << 1;
			dscal_(&i__1, &scaloc, &x[1], &c__1);
			*scale = scaloc * *scale;
		    }
		    x[j1] = v_ref(1, 1);
		    x[j2] = v_ref(2, 1);
		    x[*n + j1] = v_ref(1, 2);
		    x[*n + j2] = v_ref(2, 2);

/*                 Scale X(J1), .... to avoid overflow in   
                   updating right hand side.   

   Computing MAX */
		    d__5 = (d__1 = v_ref(1, 1), abs(d__1)) + (d__2 = v_ref(1, 
			    2), abs(d__2)), d__6 = (d__3 = v_ref(2, 1), abs(
			    d__3)) + (d__4 = v_ref(2, 2), abs(d__4));
		    xj = max(d__5,d__6);
		    if (xj > 1.) {
			rec = 1. / xj;
/* Computing MAX */
			d__1 = work[j1], d__2 = work[j2];
			if (max(d__1,d__2) > (bignum - xmax) * rec) {
			    dscal_(&n2, &rec, &x[1], &c__1);
			    *scale *= rec;
			}
		    }

/*                 Update the right-hand side. */

		    if (j1 > 1) {
			i__1 = j1 - 1;
			d__1 = -x[j1];
			daxpy_(&i__1, &d__1, &t_ref(1, j1), &c__1, &x[1], &
				c__1);
			i__1 = j1 - 1;
			d__1 = -x[j2];
			daxpy_(&i__1, &d__1, &t_ref(1, j2), &c__1, &x[1], &
				c__1);

			i__1 = j1 - 1;
			d__1 = -x[*n + j1];
			daxpy_(&i__1, &d__1, &t_ref(1, j1), &c__1, &x[*n + 1],
				 &c__1);
			i__1 = j1 - 1;
			d__1 = -x[*n + j2];
			daxpy_(&i__1, &d__1, &t_ref(1, j2), &c__1, &x[*n + 1],
				 &c__1);

			x[1] = x[1] + b[j1] * x[*n + j1] + b[j2] * x[*n + j2];
			x[*n + 1] = x[*n + 1] - b[j1] * x[j1] - b[j2] * x[j2];

			xmax = 0.;
			i__1 = j1 - 1;
			for (k = 1; k <= i__1; ++k) {
/* Computing MAX */
			    d__3 = (d__1 = x[k], abs(d__1)) + (d__2 = x[k + *
				    n], abs(d__2));
			    xmax = max(d__3,xmax);
/* L60: */
			}
		    }

		}
L70:
		;
	    }

	} else {

/*           Solve (T + iB)'*(p+iq) = c+id */

	    jnext = 1;
	    i__1 = *n;
	    for (j = 1; j <= i__1; ++j) {
		if (j < jnext) {
		    goto L80;
		}
		j1 = j;
		j2 = j;
		jnext = j + 1;
		if (j < *n) {
		    if (t_ref(j + 1, j) != 0.) {
			j2 = j + 1;
			jnext = j + 2;
		    }
		}

		if (j1 == j2) {

/*                 1 by 1 diagonal block   

                   Scale if necessary to avoid overflow in forming the   
                   right-hand side element by inner product. */

		    xj = (d__1 = x[j1], abs(d__1)) + (d__2 = x[j1 + *n], abs(
			    d__2));
		    if (xmax > 1.) {
			rec = 1. / xmax;
			if (work[j1] > (bignum - xj) * rec) {
			    dscal_(&n2, &rec, &x[1], &c__1);
			    *scale *= rec;
			    xmax *= rec;
			}
		    }

		    i__2 = j1 - 1;
		    x[j1] -= ddot_(&i__2, &t_ref(1, j1), &c__1, &x[1], &c__1);
		    i__2 = j1 - 1;
		    x[*n + j1] -= ddot_(&i__2, &t_ref(1, j1), &c__1, &x[*n + 
			    1], &c__1);
		    if (j1 > 1) {
			x[j1] -= b[j1] * x[*n + 1];
			x[*n + j1] += b[j1] * x[1];
		    }
		    xj = (d__1 = x[j1], abs(d__1)) + (d__2 = x[j1 + *n], abs(
			    d__2));

		    z__ = *w;
		    if (j1 == 1) {
			z__ = b[1];
		    }

/*                 Scale if necessary to avoid overflow in   
                   complex division */

		    tjj = (d__1 = t_ref(j1, j1), abs(d__1)) + abs(z__);
		    tmp = t_ref(j1, j1);
		    if (tjj < sminw) {
			tmp = sminw;
			tjj = sminw;
			*info = 1;
		    }

		    if (tjj < 1.) {
			if (xj > bignum * tjj) {
			    rec = 1. / xj;
			    dscal_(&n2, &rec, &x[1], &c__1);
			    *scale *= rec;
			    xmax *= rec;
			}
		    }
		    d__1 = -z__;
		    dladiv_(&x[j1], &x[*n + j1], &tmp, &d__1, &sr, &si);
		    x[j1] = sr;
		    x[j1 + *n] = si;
/* Computing MAX */
		    d__3 = (d__1 = x[j1], abs(d__1)) + (d__2 = x[j1 + *n], 
			    abs(d__2));
		    xmax = max(d__3,xmax);

		} else {

/*                 2 by 2 diagonal block   

                   Scale if necessary to avoid overflow in forming the   
                   right-hand side element by inner product.   

   Computing MAX */
		    d__5 = (d__1 = x[j1], abs(d__1)) + (d__2 = x[*n + j1], 
			    abs(d__2)), d__6 = (d__3 = x[j2], abs(d__3)) + (
			    d__4 = x[*n + j2], abs(d__4));
		    xj = max(d__5,d__6);
		    if (xmax > 1.) {
			rec = 1. / xmax;
/* Computing MAX */
			d__1 = work[j1], d__2 = work[j2];
			if (max(d__1,d__2) > (bignum - xj) / xmax) {
			    dscal_(&n2, &rec, &x[1], &c__1);
			    *scale *= rec;
			    xmax *= rec;
			}
		    }

		    i__2 = j1 - 1;
		    d___ref(1, 1) = x[j1] - ddot_(&i__2, &t_ref(1, j1), &c__1,
			     &x[1], &c__1);
		    i__2 = j1 - 1;
		    d___ref(2, 1) = x[j2] - ddot_(&i__2, &t_ref(1, j2), &c__1,
			     &x[1], &c__1);
		    i__2 = j1 - 1;
		    d___ref(1, 2) = x[*n + j1] - ddot_(&i__2, &t_ref(1, j1), &
			    c__1, &x[*n + 1], &c__1);
		    i__2 = j1 - 1;
		    d___ref(2, 2) = x[*n + j2] - ddot_(&i__2, &t_ref(1, j2), &
			    c__1, &x[*n + 1], &c__1);
		    d___ref(1, 1) = d___ref(1, 1) - b[j1] * x[*n + 1];
		    d___ref(2, 1) = d___ref(2, 1) - b[j2] * x[*n + 1];
		    d___ref(1, 2) = d___ref(1, 2) + b[j1] * x[1];
		    d___ref(2, 2) = d___ref(2, 2) + b[j2] * x[1];

		    dlaln2_(&c_true, &c__2, &c__2, &sminw, &c_b21, &t_ref(j1, 
			    j1), ldt, &c_b21, &c_b21, d__, &c__2, &c_b25, w, 
			    v, &c__2, &scaloc, &xnorm, &ierr);
		    if (ierr != 0) {
			*info = 2;
		    }

		    if (scaloc != 1.) {
			dscal_(&n2, &scaloc, &x[1], &c__1);
			*scale = scaloc * *scale;
		    }
		    x[j1] = v_ref(1, 1);
		    x[j2] = v_ref(2, 1);
		    x[*n + j1] = v_ref(1, 2);
		    x[*n + j2] = v_ref(2, 2);
/* Computing MAX */
		    d__5 = (d__1 = x[j1], abs(d__1)) + (d__2 = x[*n + j1], 
			    abs(d__2)), d__6 = (d__3 = x[j2], abs(d__3)) + (
			    d__4 = x[*n + j2], abs(d__4)), d__5 = max(d__5,
			    d__6);
		    xmax = max(d__5,xmax);

		}

L80:
		;
	    }

	}

    }

    return 0;

/*     End of DLAQTR */

} /* dlaqtr_ */
Beispiel #11
0
/* Subroutine */ int dlarfb_(char *side, char *trans, char *direct, char *
	storev, integer *m, integer *n, integer *k, doublereal *v, integer *
	ldv, doublereal *t, integer *ldt, doublereal *c__, integer *ldc, 
	doublereal *work, integer *ldwork)
{
/*  -- LAPACK auxiliary routine (version 3.0) --   
       Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,   
       Courant Institute, Argonne National Lab, and Rice University   
       February 29, 1992   


    Purpose   
    =======   

    DLARFB applies a real block reflector H or its transpose H' to a   
    real m by n matrix C, from either the left or the right.   

    Arguments   
    =========   

    SIDE    (input) CHARACTER*1   
            = 'L': apply H or H' from the Left   
            = 'R': apply H or H' from the Right   

    TRANS   (input) CHARACTER*1   
            = 'N': apply H (No transpose)   
            = 'T': apply H' (Transpose)   

    DIRECT  (input) CHARACTER*1   
            Indicates how H is formed from a product of elementary   
            reflectors   
            = 'F': H = H(1) H(2) . . . H(k) (Forward)   
            = 'B': H = H(k) . . . H(2) H(1) (Backward)   

    STOREV  (input) CHARACTER*1   
            Indicates how the vectors which define the elementary   
            reflectors are stored:   
            = 'C': Columnwise   
            = 'R': Rowwise   

    M       (input) INTEGER   
            The number of rows of the matrix C.   

    N       (input) INTEGER   
            The number of columns of the matrix C.   

    K       (input) INTEGER   
            The order of the matrix T (= the number of elementary   
            reflectors whose product defines the block reflector).   

    V       (input) DOUBLE PRECISION array, dimension   
                                  (LDV,K) if STOREV = 'C'   
                                  (LDV,M) if STOREV = 'R' and SIDE = 'L'   
                                  (LDV,N) if STOREV = 'R' and SIDE = 'R'   
            The matrix V. See further details.   

    LDV     (input) INTEGER   
            The leading dimension of the array V.   
            If STOREV = 'C' and SIDE = 'L', LDV >= max(1,M);   
            if STOREV = 'C' and SIDE = 'R', LDV >= max(1,N);   
            if STOREV = 'R', LDV >= K.   

    T       (input) DOUBLE PRECISION array, dimension (LDT,K)   
            The triangular k by k matrix T in the representation of the   
            block reflector.   

    LDT     (input) INTEGER   
            The leading dimension of the array T. LDT >= K.   

    C       (input/output) DOUBLE PRECISION array, dimension (LDC,N)   
            On entry, the m by n matrix C.   
            On exit, C is overwritten by H*C or H'*C or C*H or C*H'.   

    LDC     (input) INTEGER   
            The leading dimension of the array C. LDA >= max(1,M).   

    WORK    (workspace) DOUBLE PRECISION array, dimension (LDWORK,K)   

    LDWORK  (input) INTEGER   
            The leading dimension of the array WORK.   
            If SIDE = 'L', LDWORK >= max(1,N);   
            if SIDE = 'R', LDWORK >= max(1,M).   

    =====================================================================   


       Quick return if possible   

       Parameter adjustments */
    /* Table of constant values */
    static integer c__1 = 1;
    static doublereal c_b14 = 1.;
    static doublereal c_b25 = -1.;
    
    /* System generated locals */
    integer c_dim1, c_offset, t_dim1, t_offset, v_dim1, v_offset, work_dim1, 
	    work_offset, i__1, i__2;
    /* Local variables */
    static integer i__, j;
    extern /* Subroutine */ int dgemm_(char *, char *, integer *, integer *, 
	    integer *, doublereal *, doublereal *, integer *, doublereal *, 
	    integer *, doublereal *, doublereal *, integer *);
    extern logical lsame_(char *, char *);
    extern /* Subroutine */ int dcopy_(integer *, doublereal *, integer *, 
	    doublereal *, integer *), dtrmm_(char *, char *, char *, char *, 
	    integer *, integer *, doublereal *, doublereal *, integer *, 
	    doublereal *, integer *);
    static char transt[1];
#define work_ref(a_1,a_2) work[(a_2)*work_dim1 + a_1]
#define c___ref(a_1,a_2) c__[(a_2)*c_dim1 + a_1]
#define v_ref(a_1,a_2) v[(a_2)*v_dim1 + a_1]


    v_dim1 = *ldv;
    v_offset = 1 + v_dim1 * 1;
    v -= v_offset;
    t_dim1 = *ldt;
    t_offset = 1 + t_dim1 * 1;
    t -= t_offset;
    c_dim1 = *ldc;
    c_offset = 1 + c_dim1 * 1;
    c__ -= c_offset;
    work_dim1 = *ldwork;
    work_offset = 1 + work_dim1 * 1;
    work -= work_offset;

    /* Function Body */
    if (*m <= 0 || *n <= 0) {
	return 0;
    }

    if (lsame_(trans, "N")) {
	*(unsigned char *)transt = 'T';
    } else {
	*(unsigned char *)transt = 'N';
    }

    if (lsame_(storev, "C")) {

	if (lsame_(direct, "F")) {

/*           Let  V =  ( V1 )    (first K rows)   
                       ( V2 )   
             where  V1  is unit lower triangular. */

	    if (lsame_(side, "L")) {

/*              Form  H * C  or  H' * C  where  C = ( C1 )   
                                                    ( C2 )   

                W := C' * V  =  (C1'*V1 + C2'*V2)  (stored in WORK)   

                W := C1' */

		i__1 = *k;
		for (j = 1; j <= i__1; ++j) {
		    dcopy_(n, &c___ref(j, 1), ldc, &work_ref(1, j), &c__1);
/* L10: */
		}

/*              W := W * V1 */

		dtrmm_("Right", "Lower", "No transpose", "Unit", n, k, &c_b14,
			 &v[v_offset], ldv, &work[work_offset], ldwork);
		if (*m > *k) {

/*                 W := W + C2'*V2 */

		    i__1 = *m - *k;
		    dgemm_("Transpose", "No transpose", n, k, &i__1, &c_b14, &
			    c___ref(*k + 1, 1), ldc, &v_ref(*k + 1, 1), ldv, &
			    c_b14, &work[work_offset], ldwork);
		}

/*              W := W * T'  or  W * T */

		dtrmm_("Right", "Upper", transt, "Non-unit", n, k, &c_b14, &t[
			t_offset], ldt, &work[work_offset], ldwork);

/*              C := C - V * W' */

		if (*m > *k) {

/*                 C2 := C2 - V2 * W' */

		    i__1 = *m - *k;
		    dgemm_("No transpose", "Transpose", &i__1, n, k, &c_b25, &
			    v_ref(*k + 1, 1), ldv, &work[work_offset], ldwork,
			     &c_b14, &c___ref(*k + 1, 1), ldc);
		}

/*              W := W * V1' */

		dtrmm_("Right", "Lower", "Transpose", "Unit", n, k, &c_b14, &
			v[v_offset], ldv, &work[work_offset], ldwork);

/*              C1 := C1 - W' */

		i__1 = *k;
		for (j = 1; j <= i__1; ++j) {
		    i__2 = *n;
		    for (i__ = 1; i__ <= i__2; ++i__) {
			c___ref(j, i__) = c___ref(j, i__) - work_ref(i__, j);
/* L20: */
		    }
/* L30: */
		}

	    } else if (lsame_(side, "R")) {

/*              Form  C * H  or  C * H'  where  C = ( C1  C2 )   

                W := C * V  =  (C1*V1 + C2*V2)  (stored in WORK)   

                W := C1 */

		i__1 = *k;
		for (j = 1; j <= i__1; ++j) {
		    dcopy_(m, &c___ref(1, j), &c__1, &work_ref(1, j), &c__1);
/* L40: */
		}

/*              W := W * V1 */

		dtrmm_("Right", "Lower", "No transpose", "Unit", m, k, &c_b14,
			 &v[v_offset], ldv, &work[work_offset], ldwork);
		if (*n > *k) {

/*                 W := W + C2 * V2 */

		    i__1 = *n - *k;
		    dgemm_("No transpose", "No transpose", m, k, &i__1, &
			    c_b14, &c___ref(1, *k + 1), ldc, &v_ref(*k + 1, 1)
			    , ldv, &c_b14, &work[work_offset], ldwork);
		}

/*              W := W * T  or  W * T' */

		dtrmm_("Right", "Upper", trans, "Non-unit", m, k, &c_b14, &t[
			t_offset], ldt, &work[work_offset], ldwork);

/*              C := C - W * V' */

		if (*n > *k) {

/*                 C2 := C2 - W * V2' */

		    i__1 = *n - *k;
		    dgemm_("No transpose", "Transpose", m, &i__1, k, &c_b25, &
			    work[work_offset], ldwork, &v_ref(*k + 1, 1), ldv,
			     &c_b14, &c___ref(1, *k + 1), ldc);
		}

/*              W := W * V1' */

		dtrmm_("Right", "Lower", "Transpose", "Unit", m, k, &c_b14, &
			v[v_offset], ldv, &work[work_offset], ldwork);

/*              C1 := C1 - W */

		i__1 = *k;
		for (j = 1; j <= i__1; ++j) {
		    i__2 = *m;
		    for (i__ = 1; i__ <= i__2; ++i__) {
			c___ref(i__, j) = c___ref(i__, j) - work_ref(i__, j);
/* L50: */
		    }
/* L60: */
		}
	    }

	} else {

/*           Let  V =  ( V1 )   
                       ( V2 )    (last K rows)   
             where  V2  is unit upper triangular. */

	    if (lsame_(side, "L")) {

/*              Form  H * C  or  H' * C  where  C = ( C1 )   
                                                    ( C2 )   

                W := C' * V  =  (C1'*V1 + C2'*V2)  (stored in WORK)   

                W := C2' */

		i__1 = *k;
		for (j = 1; j <= i__1; ++j) {
		    dcopy_(n, &c___ref(*m - *k + j, 1), ldc, &work_ref(1, j), 
			    &c__1);
/* L70: */
		}

/*              W := W * V2 */

		dtrmm_("Right", "Upper", "No transpose", "Unit", n, k, &c_b14,
			 &v_ref(*m - *k + 1, 1), ldv, &work[work_offset], 
			ldwork);
		if (*m > *k) {

/*                 W := W + C1'*V1 */

		    i__1 = *m - *k;
		    dgemm_("Transpose", "No transpose", n, k, &i__1, &c_b14, &
			    c__[c_offset], ldc, &v[v_offset], ldv, &c_b14, &
			    work[work_offset], ldwork);
		}

/*              W := W * T'  or  W * T */

		dtrmm_("Right", "Lower", transt, "Non-unit", n, k, &c_b14, &t[
			t_offset], ldt, &work[work_offset], ldwork);

/*              C := C - V * W' */

		if (*m > *k) {

/*                 C1 := C1 - V1 * W' */

		    i__1 = *m - *k;
		    dgemm_("No transpose", "Transpose", &i__1, n, k, &c_b25, &
			    v[v_offset], ldv, &work[work_offset], ldwork, &
			    c_b14, &c__[c_offset], ldc)
			    ;
		}

/*              W := W * V2' */

		dtrmm_("Right", "Upper", "Transpose", "Unit", n, k, &c_b14, &
			v_ref(*m - *k + 1, 1), ldv, &work[work_offset], 
			ldwork);

/*              C2 := C2 - W' */

		i__1 = *k;
		for (j = 1; j <= i__1; ++j) {
		    i__2 = *n;
		    for (i__ = 1; i__ <= i__2; ++i__) {
			c___ref(*m - *k + j, i__) = c___ref(*m - *k + j, i__) 
				- work_ref(i__, j);
/* L80: */
		    }
/* L90: */
		}

	    } else if (lsame_(side, "R")) {

/*              Form  C * H  or  C * H'  where  C = ( C1  C2 )   

                W := C * V  =  (C1*V1 + C2*V2)  (stored in WORK)   

                W := C2 */

		i__1 = *k;
		for (j = 1; j <= i__1; ++j) {
		    dcopy_(m, &c___ref(1, *n - *k + j), &c__1, &work_ref(1, j)
			    , &c__1);
/* L100: */
		}

/*              W := W * V2 */

		dtrmm_("Right", "Upper", "No transpose", "Unit", m, k, &c_b14,
			 &v_ref(*n - *k + 1, 1), ldv, &work[work_offset], 
			ldwork);
		if (*n > *k) {

/*                 W := W + C1 * V1 */

		    i__1 = *n - *k;
		    dgemm_("No transpose", "No transpose", m, k, &i__1, &
			    c_b14, &c__[c_offset], ldc, &v[v_offset], ldv, &
			    c_b14, &work[work_offset], ldwork);
		}

/*              W := W * T  or  W * T' */

		dtrmm_("Right", "Lower", trans, "Non-unit", m, k, &c_b14, &t[
			t_offset], ldt, &work[work_offset], ldwork);

/*              C := C - W * V' */

		if (*n > *k) {

/*                 C1 := C1 - W * V1' */

		    i__1 = *n - *k;
		    dgemm_("No transpose", "Transpose", m, &i__1, k, &c_b25, &
			    work[work_offset], ldwork, &v[v_offset], ldv, &
			    c_b14, &c__[c_offset], ldc)
			    ;
		}

/*              W := W * V2' */

		dtrmm_("Right", "Upper", "Transpose", "Unit", m, k, &c_b14, &
			v_ref(*n - *k + 1, 1), ldv, &work[work_offset], 
			ldwork);

/*              C2 := C2 - W */

		i__1 = *k;
		for (j = 1; j <= i__1; ++j) {
		    i__2 = *m;
		    for (i__ = 1; i__ <= i__2; ++i__) {
			c___ref(i__, *n - *k + j) = c___ref(i__, *n - *k + j) 
				- work_ref(i__, j);
/* L110: */
		    }
/* L120: */
		}
	    }
	}

    } else if (lsame_(storev, "R")) {

	if (lsame_(direct, "F")) {

/*           Let  V =  ( V1  V2 )    (V1: first K columns)   
             where  V1  is unit upper triangular. */

	    if (lsame_(side, "L")) {

/*              Form  H * C  or  H' * C  where  C = ( C1 )   
                                                    ( C2 )   

                W := C' * V'  =  (C1'*V1' + C2'*V2') (stored in WORK)   

                W := C1' */

		i__1 = *k;
		for (j = 1; j <= i__1; ++j) {
		    dcopy_(n, &c___ref(j, 1), ldc, &work_ref(1, j), &c__1);
/* L130: */
		}

/*              W := W * V1' */

		dtrmm_("Right", "Upper", "Transpose", "Unit", n, k, &c_b14, &
			v[v_offset], ldv, &work[work_offset], ldwork);
		if (*m > *k) {

/*                 W := W + C2'*V2' */

		    i__1 = *m - *k;
		    dgemm_("Transpose", "Transpose", n, k, &i__1, &c_b14, &
			    c___ref(*k + 1, 1), ldc, &v_ref(1, *k + 1), ldv, &
			    c_b14, &work[work_offset], ldwork);
		}

/*              W := W * T'  or  W * T */

		dtrmm_("Right", "Upper", transt, "Non-unit", n, k, &c_b14, &t[
			t_offset], ldt, &work[work_offset], ldwork);

/*              C := C - V' * W' */

		if (*m > *k) {

/*                 C2 := C2 - V2' * W' */

		    i__1 = *m - *k;
		    dgemm_("Transpose", "Transpose", &i__1, n, k, &c_b25, &
			    v_ref(1, *k + 1), ldv, &work[work_offset], ldwork,
			     &c_b14, &c___ref(*k + 1, 1), ldc);
		}

/*              W := W * V1 */

		dtrmm_("Right", "Upper", "No transpose", "Unit", n, k, &c_b14,
			 &v[v_offset], ldv, &work[work_offset], ldwork);

/*              C1 := C1 - W' */

		i__1 = *k;
		for (j = 1; j <= i__1; ++j) {
		    i__2 = *n;
		    for (i__ = 1; i__ <= i__2; ++i__) {
			c___ref(j, i__) = c___ref(j, i__) - work_ref(i__, j);
/* L140: */
		    }
/* L150: */
		}

	    } else if (lsame_(side, "R")) {

/*              Form  C * H  or  C * H'  where  C = ( C1  C2 )   

                W := C * V'  =  (C1*V1' + C2*V2')  (stored in WORK)   

                W := C1 */

		i__1 = *k;
		for (j = 1; j <= i__1; ++j) {
		    dcopy_(m, &c___ref(1, j), &c__1, &work_ref(1, j), &c__1);
/* L160: */
		}

/*              W := W * V1' */

		dtrmm_("Right", "Upper", "Transpose", "Unit", m, k, &c_b14, &
			v[v_offset], ldv, &work[work_offset], ldwork);
		if (*n > *k) {

/*                 W := W + C2 * V2' */

		    i__1 = *n - *k;
		    dgemm_("No transpose", "Transpose", m, k, &i__1, &c_b14, &
			    c___ref(1, *k + 1), ldc, &v_ref(1, *k + 1), ldv, &
			    c_b14, &work[work_offset], ldwork);
		}

/*              W := W * T  or  W * T' */

		dtrmm_("Right", "Upper", trans, "Non-unit", m, k, &c_b14, &t[
			t_offset], ldt, &work[work_offset], ldwork);

/*              C := C - W * V */

		if (*n > *k) {

/*                 C2 := C2 - W * V2 */

		    i__1 = *n - *k;
		    dgemm_("No transpose", "No transpose", m, &i__1, k, &
			    c_b25, &work[work_offset], ldwork, &v_ref(1, *k + 
			    1), ldv, &c_b14, &c___ref(1, *k + 1), ldc);
		}

/*              W := W * V1 */

		dtrmm_("Right", "Upper", "No transpose", "Unit", m, k, &c_b14,
			 &v[v_offset], ldv, &work[work_offset], ldwork);

/*              C1 := C1 - W */

		i__1 = *k;
		for (j = 1; j <= i__1; ++j) {
		    i__2 = *m;
		    for (i__ = 1; i__ <= i__2; ++i__) {
			c___ref(i__, j) = c___ref(i__, j) - work_ref(i__, j);
/* L170: */
		    }
/* L180: */
		}

	    }

	} else {

/*           Let  V =  ( V1  V2 )    (V2: last K columns)   
             where  V2  is unit lower triangular. */

	    if (lsame_(side, "L")) {

/*              Form  H * C  or  H' * C  where  C = ( C1 )   
                                                    ( C2 )   

                W := C' * V'  =  (C1'*V1' + C2'*V2') (stored in WORK)   

                W := C2' */

		i__1 = *k;
		for (j = 1; j <= i__1; ++j) {
		    dcopy_(n, &c___ref(*m - *k + j, 1), ldc, &work_ref(1, j), 
			    &c__1);
/* L190: */
		}

/*              W := W * V2' */

		dtrmm_("Right", "Lower", "Transpose", "Unit", n, k, &c_b14, &
			v_ref(1, *m - *k + 1), ldv, &work[work_offset], 
			ldwork);
		if (*m > *k) {

/*                 W := W + C1'*V1' */

		    i__1 = *m - *k;
		    dgemm_("Transpose", "Transpose", n, k, &i__1, &c_b14, &
			    c__[c_offset], ldc, &v[v_offset], ldv, &c_b14, &
			    work[work_offset], ldwork);
		}

/*              W := W * T'  or  W * T */

		dtrmm_("Right", "Lower", transt, "Non-unit", n, k, &c_b14, &t[
			t_offset], ldt, &work[work_offset], ldwork);

/*              C := C - V' * W' */

		if (*m > *k) {

/*                 C1 := C1 - V1' * W' */

		    i__1 = *m - *k;
		    dgemm_("Transpose", "Transpose", &i__1, n, k, &c_b25, &v[
			    v_offset], ldv, &work[work_offset], ldwork, &
			    c_b14, &c__[c_offset], ldc);
		}

/*              W := W * V2 */

		dtrmm_("Right", "Lower", "No transpose", "Unit", n, k, &c_b14,
			 &v_ref(1, *m - *k + 1), ldv, &work[work_offset], 
			ldwork);

/*              C2 := C2 - W' */

		i__1 = *k;
		for (j = 1; j <= i__1; ++j) {
		    i__2 = *n;
		    for (i__ = 1; i__ <= i__2; ++i__) {
			c___ref(*m - *k + j, i__) = c___ref(*m - *k + j, i__) 
				- work_ref(i__, j);
/* L200: */
		    }
/* L210: */
		}

	    } else if (lsame_(side, "R")) {

/*              Form  C * H  or  C * H'  where  C = ( C1  C2 )   

                W := C * V'  =  (C1*V1' + C2*V2')  (stored in WORK)   

                W := C2 */

		i__1 = *k;
		for (j = 1; j <= i__1; ++j) {
		    dcopy_(m, &c___ref(1, *n - *k + j), &c__1, &work_ref(1, j)
			    , &c__1);
/* L220: */
		}

/*              W := W * V2' */

		dtrmm_("Right", "Lower", "Transpose", "Unit", m, k, &c_b14, &
			v_ref(1, *n - *k + 1), ldv, &work[work_offset], 
			ldwork);
		if (*n > *k) {

/*                 W := W + C1 * V1' */

		    i__1 = *n - *k;
		    dgemm_("No transpose", "Transpose", m, k, &i__1, &c_b14, &
			    c__[c_offset], ldc, &v[v_offset], ldv, &c_b14, &
			    work[work_offset], ldwork);
		}

/*              W := W * T  or  W * T' */

		dtrmm_("Right", "Lower", trans, "Non-unit", m, k, &c_b14, &t[
			t_offset], ldt, &work[work_offset], ldwork);

/*              C := C - W * V */

		if (*n > *k) {

/*                 C1 := C1 - W * V1 */

		    i__1 = *n - *k;
		    dgemm_("No transpose", "No transpose", m, &i__1, k, &
			    c_b25, &work[work_offset], ldwork, &v[v_offset], 
			    ldv, &c_b14, &c__[c_offset], ldc);
		}

/*              W := W * V2 */

		dtrmm_("Right", "Lower", "No transpose", "Unit", m, k, &c_b14,
			 &v_ref(1, *n - *k + 1), ldv, &work[work_offset], 
			ldwork);

/*              C1 := C1 - W */

		i__1 = *k;
		for (j = 1; j <= i__1; ++j) {
		    i__2 = *m;
		    for (i__ = 1; i__ <= i__2; ++i__) {
			c___ref(i__, *n - *k + j) = c___ref(i__, *n - *k + j) 
				- work_ref(i__, j);
/* L230: */
		    }
/* L240: */
		}

	    }

	}
    }

    return 0;

/*     End of DLARFB */

} /* dlarfb_ */
Beispiel #12
0
/* Subroutine */ int cunt03_(char *rc, integer *mu, integer *mv, integer *n, 
	integer *k, complex *u, integer *ldu, complex *v, integer *ldv, 
	complex *work, integer *lwork, real *rwork, real *result, integer *
	info)
{
    /* System generated locals */
    integer u_dim1, u_offset, v_dim1, v_offset, i__1, i__2, i__3, i__4;
    real r__1, r__2;
    complex q__1, q__2;

    /* Builtin functions */
    double c_abs(complex *);
    void c_div(complex *, complex *, complex *);

    /* Local variables */
    static integer i__, j;
    static complex s;
    extern logical lsame_(char *, char *);
    extern /* Subroutine */ int cunt01_(char *, integer *, integer *, complex 
	    *, integer *, complex *, integer *, real *, real *);
    extern integer icamax_(integer *, complex *, integer *);
    extern doublereal slamch_(char *);
    static complex su, sv;
    extern /* Subroutine */ int xerbla_(char *, integer *);
    static integer irc, lmx;
    static real ulp, res1, res2;


#define u_subscr(a_1,a_2) (a_2)*u_dim1 + a_1
#define u_ref(a_1,a_2) u[u_subscr(a_1,a_2)]
#define v_subscr(a_1,a_2) (a_2)*v_dim1 + a_1
#define v_ref(a_1,a_2) v[v_subscr(a_1,a_2)]


/*  -- LAPACK test routine (version 3.0) --   
       Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,   
       Courant Institute, Argonne National Lab, and Rice University   
       February 29, 1992   


    Purpose   
    =======   

    CUNT03 compares two unitary matrices U and V to see if their   
    corresponding rows or columns span the same spaces.  The rows are   
    checked if RC = 'R', and the columns are checked if RC = 'C'.   

    RESULT is the maximum of   

       | V*V' - I | / ( MV ulp ), if RC = 'R', or   

       | V'*V - I | / ( MV ulp ), if RC = 'C',   

    and the maximum over rows (or columns) 1 to K of   

       | U(i) - S*V(i) |/ ( N ulp )   

    where abs(S) = 1 (chosen to minimize the expression), U(i) is the   
    i-th row (column) of U, and V(i) is the i-th row (column) of V.   

    Arguments   
    ==========   

    RC      (input) CHARACTER*1   
            If RC = 'R' the rows of U and V are to be compared.   
            If RC = 'C' the columns of U and V are to be compared.   

    MU      (input) INTEGER   
            The number of rows of U if RC = 'R', and the number of   
            columns if RC = 'C'.  If MU = 0 CUNT03 does nothing.   
            MU must be at least zero.   

    MV      (input) INTEGER   
            The number of rows of V if RC = 'R', and the number of   
            columns if RC = 'C'.  If MV = 0 CUNT03 does nothing.   
            MV must be at least zero.   

    N       (input) INTEGER   
            If RC = 'R', the number of columns in the matrices U and V,   
            and if RC = 'C', the number of rows in U and V.  If N = 0   
            CUNT03 does nothing.  N must be at least zero.   

    K       (input) INTEGER   
            The number of rows or columns of U and V to compare.   
            0 <= K <= max(MU,MV).   

    U       (input) COMPLEX array, dimension (LDU,N)   
            The first matrix to compare.  If RC = 'R', U is MU by N, and   
            if RC = 'C', U is N by MU.   

    LDU     (input) INTEGER   
            The leading dimension of U.  If RC = 'R', LDU >= max(1,MU),   
            and if RC = 'C', LDU >= max(1,N).   

    V       (input) COMPLEX array, dimension (LDV,N)   
            The second matrix to compare.  If RC = 'R', V is MV by N, and   
            if RC = 'C', V is N by MV.   

    LDV     (input) INTEGER   
            The leading dimension of V.  If RC = 'R', LDV >= max(1,MV),   
            and if RC = 'C', LDV >= max(1,N).   

    WORK    (workspace) COMPLEX array, dimension (LWORK)   

    LWORK   (input) INTEGER   
            The length of the array WORK.  For best performance, LWORK   
            should be at least N*N if RC = 'C' or M*M if RC = 'R', but   
            the tests will be done even if LWORK is 0.   

    RWORK   (workspace) REAL array, dimension (max(MV,N))   

    RESULT  (output) REAL   
            The value computed by the test described above.  RESULT is   
            limited to 1/ulp to avoid overflow.   

    INFO    (output) INTEGER   
            0  indicates a successful exit   
            -k indicates the k-th parameter had an illegal value   

    =====================================================================   



       Check inputs   

       Parameter adjustments */
    u_dim1 = *ldu;
    u_offset = 1 + u_dim1 * 1;
    u -= u_offset;
    v_dim1 = *ldv;
    v_offset = 1 + v_dim1 * 1;
    v -= v_offset;
    --work;
    --rwork;

    /* Function Body */
    *info = 0;
    if (lsame_(rc, "R")) {
	irc = 0;
    } else if (lsame_(rc, "C")) {
	irc = 1;
    } else {
	irc = -1;
    }
    if (irc == -1) {
	*info = -1;
    } else if (*mu < 0) {
	*info = -2;
    } else if (*mv < 0) {
	*info = -3;
    } else if (*n < 0) {
	*info = -4;
    } else if (*k < 0 || *k > max(*mu,*mv)) {
	*info = -5;
    } else if (irc == 0 && *ldu < max(1,*mu) || irc == 1 && *ldu < max(1,*n)) 
	    {
	*info = -7;
    } else if (irc == 0 && *ldv < max(1,*mv) || irc == 1 && *ldv < max(1,*n)) 
	    {
	*info = -9;
    }
    if (*info != 0) {
	i__1 = -(*info);
	xerbla_("CUNT03", &i__1);
	return 0;
    }

/*     Initialize result */

    *result = 0.f;
    if (*mu == 0 || *mv == 0 || *n == 0) {
	return 0;
    }

/*     Machine constants */

    ulp = slamch_("Precision");

    if (irc == 0) {

/*        Compare rows */

	res1 = 0.f;
	i__1 = *k;
	for (i__ = 1; i__ <= i__1; ++i__) {
	    lmx = icamax_(n, &u_ref(i__, 1), ldu);
	    i__2 = v_subscr(i__, lmx);
	    if (v[i__2].r == 0.f && v[i__2].i == 0.f) {
		sv.r = 1.f, sv.i = 0.f;
	    } else {
		r__1 = c_abs(&v_ref(i__, lmx));
		q__2.r = r__1, q__2.i = 0.f;
		c_div(&q__1, &q__2, &v_ref(i__, lmx));
		sv.r = q__1.r, sv.i = q__1.i;
	    }
	    i__2 = u_subscr(i__, lmx);
	    if (u[i__2].r == 0.f && u[i__2].i == 0.f) {
		su.r = 1.f, su.i = 0.f;
	    } else {
		r__1 = c_abs(&u_ref(i__, lmx));
		q__2.r = r__1, q__2.i = 0.f;
		c_div(&q__1, &q__2, &u_ref(i__, lmx));
		su.r = q__1.r, su.i = q__1.i;
	    }
	    c_div(&q__1, &sv, &su);
	    s.r = q__1.r, s.i = q__1.i;
	    i__2 = *n;
	    for (j = 1; j <= i__2; ++j) {
/* Computing MAX */
		i__3 = u_subscr(i__, j);
		i__4 = v_subscr(i__, j);
		q__2.r = s.r * v[i__4].r - s.i * v[i__4].i, q__2.i = s.r * v[
			i__4].i + s.i * v[i__4].r;
		q__1.r = u[i__3].r - q__2.r, q__1.i = u[i__3].i - q__2.i;
		r__1 = res1, r__2 = c_abs(&q__1);
		res1 = dmax(r__1,r__2);
/* L10: */
	    }
/* L20: */
	}
	res1 /= (real) (*n) * ulp;

/*        Compute orthogonality of rows of V. */

	cunt01_("Rows", mv, n, &v[v_offset], ldv, &work[1], lwork, &rwork[1], 
		&res2);

    } else {

/*        Compare columns */

	res1 = 0.f;
	i__1 = *k;
	for (i__ = 1; i__ <= i__1; ++i__) {
	    lmx = icamax_(n, &u_ref(1, i__), &c__1);
	    i__2 = v_subscr(lmx, i__);
	    if (v[i__2].r == 0.f && v[i__2].i == 0.f) {
		sv.r = 1.f, sv.i = 0.f;
	    } else {
		r__1 = c_abs(&v_ref(lmx, i__));
		q__2.r = r__1, q__2.i = 0.f;
		c_div(&q__1, &q__2, &v_ref(lmx, i__));
		sv.r = q__1.r, sv.i = q__1.i;
	    }
	    i__2 = u_subscr(lmx, i__);
	    if (u[i__2].r == 0.f && u[i__2].i == 0.f) {
		su.r = 1.f, su.i = 0.f;
	    } else {
		r__1 = c_abs(&u_ref(lmx, i__));
		q__2.r = r__1, q__2.i = 0.f;
		c_div(&q__1, &q__2, &u_ref(lmx, i__));
		su.r = q__1.r, su.i = q__1.i;
	    }
	    c_div(&q__1, &sv, &su);
	    s.r = q__1.r, s.i = q__1.i;
	    i__2 = *n;
	    for (j = 1; j <= i__2; ++j) {
/* Computing MAX */
		i__3 = u_subscr(j, i__);
		i__4 = v_subscr(j, i__);
		q__2.r = s.r * v[i__4].r - s.i * v[i__4].i, q__2.i = s.r * v[
			i__4].i + s.i * v[i__4].r;
		q__1.r = u[i__3].r - q__2.r, q__1.i = u[i__3].i - q__2.i;
		r__1 = res1, r__2 = c_abs(&q__1);
		res1 = dmax(r__1,r__2);
/* L30: */
	    }
/* L40: */
	}
	res1 /= (real) (*n) * ulp;

/*        Compute orthogonality of columns of V. */

	cunt01_("Columns", n, mv, &v[v_offset], ldv, &work[1], lwork, &rwork[
		1], &res2);
    }

/* Computing MIN */
    r__1 = dmax(res1,res2), r__2 = 1.f / ulp;
    *result = dmin(r__1,r__2);
    return 0;

/*     End of CUNT03 */

} /* cunt03_ */
void Controller_StateMachine_v2::processTurn() {	// "during:"

	// 1º proyectar posicion sobre el plano r_ur, r_ur2
	Vector aux_ur2(3), aux_vector(3);
	aux_vector.copy(&r_ur);
	jesus_library::multiplyDoubleVsVector( jesus_library::dotProduct(r_ur, r_ur2), aux_vector );
	aux_ur2.substraction( &r_ur2, &aux_vector);
	jesus_library::unitarizeVector(aux_ur2); // vector ortogonal a r_ur; para formar base ortonormal en el plano r_ur, r_ur2

	Vector pos_act(3);
	pos_act.setValueData(xei,1);
	pos_act.setValueData(yei,2);
	pos_act.setValueData(zei,3);

	aux_vector.substraction( &pos_act, &c_pc);
	Vector pos_act_proy_ur(3),pos_act_proy_ur2(3);
	pos_act_proy_ur.copy(&r_ur);
	pos_act_proy_ur2.copy(&aux_ur2);

	jesus_library::multiplyDoubleVsVector( jesus_library::dotProduct(r_ur, aux_vector), pos_act_proy_ur );
	jesus_library::multiplyDoubleVsVector( jesus_library::dotProduct(aux_ur2, aux_vector), pos_act_proy_ur2 );

	Vector pos_act_proy(3);
	pos_act_proy.addition( &pos_act_proy_ur, &pos_act_proy_ur2);
	cvg_double current_radius = jesus_library::normOfVector(pos_act_proy);

	#ifdef SM_STATEMACHINE_DEBUG
	std::cout << "c_pc = \n"; c_pc.mostrar();
	std::cout << "c_pinit = \n"; c_pinit.mostrar();
	std::cout << "c_pend = \n"; c_pend.mostrar();
	std::cout << "pos_act_proy = \n"; pos_act_proy.mostrar();
	#endif // SM_STATEMACHINE_DEBUG

	// 2º localizar pref
	Vector u_ro(3);
	u_ro.copy(&pos_act_proy);
	jesus_library::unitarizeVector(u_ro);
	aux_vector.copy(&u_ro);
	jesus_library::multiplyDoubleVsVector( c_Rt, aux_vector);
	Vector pos_ref(3);
	pos_ref.addition(&c_pc, &aux_vector);
	jesus_library::getVectorComponents(pos_ref, xrefo, yrefo, zrefo);
	cvg_double current_altitude_error = (zei-zrefo); // fabs(zei-zrefo);

	// 3º calcular velocidad de referencia
	Vector u_fi(3);
	jesus_library::crossProduct(u_fi, c_u0, u_ro);
	jesus_library::unitarizeVector(u_fi);
	Vector v_ref(3);
	v_ref.copy(&u_fi);
	jesus_library::multiplyDoubleVsVector(c_vc, v_ref);
	jesus_library::getVectorComponents(v_ref, vxfo, vyfo, vzfo);


//	// 4º Calcular pitcho, rollo
	derivBlock_vxfo.setInput( vxfo);
	dvxfo = derivBlock_vxfo.getOutput();
	derivBlock_vyfo.setInput( vyfo);
	dvyfo = derivBlock_vyfo.getOutput();
#ifdef SM_TRAJECTORYMODE_ACTIVATE_TILTFO
	double g = 9.81;
	pitchfo  = -asin( (cvg_double) dvxfo/g);
	rollfo   =  asin( (cvg_double) dvyfo/g);
	pitchfo *= 1/SM_TRAJECTORYMODE_TILTFO_RAD2TILTREF;
	rollfo  *= 1/SM_TRAJECTORYMODE_TILTFO_RAD2TILTREF;
#else
	pitchfo = 0.0;
	rollfo  = 0.0;
#endif
//  // This is a code that never work that was intended to stop the parrot in it's current position
//	double v_act = sqrt( vxei*vxei + vyei*vyei + vzei*vzei );
//	double act_req  = pow(v_act,2)/c_Rt;
//	double tilt_req = act_req/SM_STATEMACHINE_G;
//	aux_vector.copy(&u_ro);
//	jesus_library::multiplyDoubleVsVector( -tilt_req, aux_vector);
//	aux_vector.setValueData( 0.0, 3);
//
//	Vector parrot_ux(3), parrot_uy(3);
//	parrot_ux.setValueData( cos(yawei), 1);
//	parrot_ux.setValueData( sin(yawei), 2);
//	parrot_uy.setValueData(-sin(yawei), 1);
//	parrot_uy.setValueData( cos(yawei), 2);
//
//	pitchfo = -jesus_library::dotProduct( parrot_ux, aux_vector)*SM_STATEMACHINE_TILTCOMM_CORRECT_FACTOR;
//	rollfo  =  jesus_library::dotProduct( parrot_uy, aux_vector)*SM_STATEMACHINE_TILTCOMM_CORRECT_FACTOR;
//	pitchfo /= SM_STATEMACHINE_TILTCOMM_NORMALIZATION_CONSTANT;
//	rollfo  /= SM_STATEMACHINE_TILTCOMM_NORMALIZATION_CONSTANT;

	// 5º Calculo de alpha y comparacion con c_alim
	Vector aux_vector2(3);
	aux_vector.substraction(&c_pinit,&c_pc);
	jesus_library::unitarizeVector(aux_vector);
	cvg_double aux = jesus_library::dotProduct(u_ro, aux_vector);
	jesus_library::saturate( aux, -1, 1);
	jesus_library::crossProduct(aux_vector2, aux_vector, u_ro);
	cvg_double angle_sign = ( jesus_library::dotProduct(c_u0, aux_vector2) > 0.0 ) ? +1.0 : -1.0;
	cvg_double current_alpha = angle_sign*acos(aux);

	#ifdef SM_STATEMACHINE_DEBUG
	std::cout << "c_alim = " << c_alim << "; alpha = " << current_alpha << "\n";
	#endif // SM_STATEMACHINE_DEBUG

	if ( current_alpha > c_alim) {  // ended the turn
		c_nextState = SM_stateNames::STRAIGHT;
		c_changeState = true;
		return;
	} else {  // turn not ended, check safety zones using: current_radius, current_alpha
		if ( ( current_alpha < trajectory.traj_config.turnmode_safetyzone_negalpha_rad ) ||
				( fabs(current_radius - c_Rt) > trajectory.traj_config.turnmode_safetyzone_radius_m ) ||
				( fabs(current_altitude_error) > trajectory.traj_config.turnmode_safetyzone_altitude_m ) ) { // enter hover to prev checkpoint
#ifdef SM_STATEMACHINE_DEBUG
			std::cout << "pos_act_proy_ur < 0\n";
#endif // SM_STATEMACHINE_DEBUG
			trajectory[pr_checkpoint].convert2Vector(h_checkpoint);
			c_nextState = SM_stateNames::HOVER;
			h_stay_in_last_checkpoint = false;
			c_changeState = true;

			// In this case I have to redefine the state machine output
			jesus_library::getVectorComponents(h_checkpoint, xrefo, yrefo, zrefo);
			vxfo = 0.0;
			vyfo = 0.0;
			vzfo = 0.0;
			pitchfo = 0.0;
			rollfo  = 0.0;

			return;
		} else { // Continue turn, nothing else to do
			return;
		}
	}

}