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() );
}
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);
}
/* 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_ */
/* 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_ */
/* 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_ */
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;
}
/* 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_ */
/* 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_ */
/* 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;
}
}
}