void CExampleApp::stepMySimulation()
{
sutil::CSystemClock::tick(db_->sim_dt_);//Tick the clock.
//Update the operational point tasks (if any)
std::vector<SUiCtrlPointData>::iterator it,ite;
if(db_->s_gui_.ui_flag_[5])
{
//Press 5 to give the keyboard control
for(it = taskvec_ui_ctrl_point_.begin(), ite = taskvec_ui_ctrl_point_.end(); it!=ite; ++it )
{
it->task_->setGoalPos(db_->s_gui_.ui_point_[it->ui_pt_]);
} //Set the goal position.
}
else
{ // NOTE : If the ui flag 5 is false, we always send zero torques to the robot...
//Press 5 again to set the control point to the robot's current position
static Eigen::VectorXd tmp_vec;
for(it = taskvec_ui_ctrl_point_.begin(), ite = taskvec_ui_ctrl_point_.end(); it!=ite; ++it )
{
it->task_->getPos(tmp_vec);
db_->s_gui_.ui_point_[it->ui_pt_] << tmp_vec(0),tmp_vec(1),tmp_vec(2);
} //Get the goal position.
}
if(ctrl_ctr_%20 == 0)
{ // Every 2ms
//Set the positions of the ui points
for(it = taskvec_ui_ctrl_point_.begin(), ite = taskvec_ui_ctrl_point_.end(); it!=ite; ++it )
{
Eigen::Vector3d& tmp_ref = db_->s_gui_.ui_point_[it->ui_pt_];
it->chai_pos_des_->setLocalPos(tmp_ref(0),tmp_ref(1),tmp_ref(2));
it->task_->getPos(it->pos_);
Eigen::VectorXd& tmp_ref2 = it->pos_;
it->chai_pos_->setLocalPos(tmp_ref2(0),tmp_ref2(1),tmp_ref2(2));
}
}
if(true == has_communicated_with_robot_once_) {
robot_.computeDynamics();
robot_.computeNonControlOperations();
robot_.computeServo();
has_updated_dyn_after_robot_comm_ = true;
}
else {
robot_.computeDynamics();
robot_.computeNonControlOperations();
robot_.computeServo();
}
ctrl_ctr_++;//Increment the counter for dynamics computed.
}
/* Subroutine */ int zget36_(doublereal *rmax, integer *lmax, integer *ninfo,
integer *knt, integer *nin)
{
/* System generated locals */
integer i__1, i__2, i__3, i__4;
/* Builtin functions */
integer s_rsle(cilist *), do_lio(integer *, integer *, char *, ftnlen),
e_rsle(void);
/* Local variables */
static doublecomplex diag[10];
static integer ifst, ilst;
static doublecomplex work[200];
static integer info1, info2, i__, j, n;
static doublecomplex q[100] /* was [10][10] */, ctemp;
extern /* Subroutine */ int zhst01_(integer *, integer *, integer *,
doublecomplex *, integer *, doublecomplex *, integer *,
doublecomplex *, integer *, doublecomplex *, integer *,
doublereal *, doublereal *);
static doublereal rwork[10];
static doublecomplex t1[100] /* was [10][10] */, t2[100] /*
was [10][10] */;
extern /* Subroutine */ int zcopy_(integer *, doublecomplex *, integer *,
doublecomplex *, integer *);
extern doublereal dlamch_(char *);
extern /* Subroutine */ int zlacpy_(char *, integer *, integer *,
doublecomplex *, integer *, doublecomplex *, integer *),
zlaset_(char *, integer *, integer *, doublecomplex *,
doublecomplex *, doublecomplex *, integer *);
static doublereal result[2];
extern /* Subroutine */ int ztrexc_(char *, integer *, doublecomplex *,
integer *, doublecomplex *, integer *, integer *, integer *,
integer *);
static doublereal eps, res;
static doublecomplex tmp[100] /* was [10][10] */;
/* Fortran I/O blocks */
static cilist io___2 = { 0, 0, 0, 0, 0 };
static cilist io___7 = { 0, 0, 0, 0, 0 };
#define q_subscr(a_1,a_2) (a_2)*10 + a_1 - 11
#define q_ref(a_1,a_2) q[q_subscr(a_1,a_2)]
#define t1_subscr(a_1,a_2) (a_2)*10 + a_1 - 11
#define t1_ref(a_1,a_2) t1[t1_subscr(a_1,a_2)]
#define t2_subscr(a_1,a_2) (a_2)*10 + a_1 - 11
#define t2_ref(a_1,a_2) t2[t2_subscr(a_1,a_2)]
#define tmp_subscr(a_1,a_2) (a_2)*10 + a_1 - 11
#define tmp_ref(a_1,a_2) tmp[tmp_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
September 30, 1994
Purpose
=======
ZGET36 tests ZTREXC, a routine for reordering diagonal entries of a
matrix in complex Schur form. Thus, ZLAEXC computes a unitary matrix
Q such that
Q' * T1 * Q = T2
and where one of the diagonal blocks of T1 (the one at row IFST) has
been moved to position ILST.
The test code verifies that the residual Q'*T1*Q-T2 is small, that T2
is in Schur form, and that the final position of the IFST block is
ILST.
The test matrices are read from a file with logical unit number NIN.
Arguments
==========
RMAX (output) DOUBLE PRECISION
Value of the largest test ratio.
LMAX (output) INTEGER
Example number where largest test ratio achieved.
NINFO (output) INTEGER
Number of examples where INFO is nonzero.
KNT (output) INTEGER
Total number of examples tested.
NIN (input) INTEGER
Input logical unit number.
===================================================================== */
eps = dlamch_("P");
*rmax = 0.;
*lmax = 0;
*knt = 0;
*ninfo = 0;
/* Read input data until N=0 */
L10:
io___2.ciunit = *nin;
s_rsle(&io___2);
do_lio(&c__3, &c__1, (char *)&n, (ftnlen)sizeof(integer));
do_lio(&c__3, &c__1, (char *)&ifst, (ftnlen)sizeof(integer));
do_lio(&c__3, &c__1, (char *)&ilst, (ftnlen)sizeof(integer));
e_rsle();
if (n == 0) {
return 0;
}
++(*knt);
i__1 = n;
for (i__ = 1; i__ <= i__1; ++i__) {
io___7.ciunit = *nin;
s_rsle(&io___7);
i__2 = n;
for (j = 1; j <= i__2; ++j) {
do_lio(&c__7, &c__1, (char *)&tmp_ref(i__, j), (ftnlen)sizeof(
doublecomplex));
}
e_rsle();
/* L20: */
}
zlacpy_("F", &n, &n, tmp, &c__10, t1, &c__10);
zlacpy_("F", &n, &n, tmp, &c__10, t2, &c__10);
res = 0.;
/* Test without accumulating Q */
zlaset_("Full", &n, &n, &c_b1, &c_b2, q, &c__10);
ztrexc_("N", &n, t1, &c__10, q, &c__10, &ifst, &ilst, &info1);
i__1 = n;
for (i__ = 1; i__ <= i__1; ++i__) {
i__2 = n;
for (j = 1; j <= i__2; ++j) {
i__3 = q_subscr(i__, j);
if (i__ == j && (q[i__3].r != 1. || q[i__3].i != 0.)) {
res += 1. / eps;
}
i__3 = q_subscr(i__, j);
if (i__ != j && (q[i__3].r != 0. || q[i__3].i != 0.)) {
res += 1. / eps;
}
/* L30: */
}
/* L40: */
}
/* Test with accumulating Q */
zlaset_("Full", &n, &n, &c_b1, &c_b2, q, &c__10);
ztrexc_("V", &n, t2, &c__10, q, &c__10, &ifst, &ilst, &info2);
/* Compare T1 with T2 */
i__1 = n;
for (i__ = 1; i__ <= i__1; ++i__) {
i__2 = n;
for (j = 1; j <= i__2; ++j) {
i__3 = t1_subscr(i__, j);
i__4 = t2_subscr(i__, j);
if (t1[i__3].r != t2[i__4].r || t1[i__3].i != t2[i__4].i) {
res += 1. / eps;
}
/* L50: */
}
/* L60: */
}
if (info1 != 0 || info2 != 0) {
++(*ninfo);
}
if (info1 != info2) {
res += 1. / eps;
}
/* Test for successful reordering of T2 */
zcopy_(&n, tmp, &c__11, diag, &c__1);
if (ifst < ilst) {
i__1 = ilst;
for (i__ = ifst + 1; i__ <= i__1; ++i__) {
i__2 = i__ - 1;
ctemp.r = diag[i__2].r, ctemp.i = diag[i__2].i;
i__2 = i__ - 1;
i__3 = i__ - 2;
diag[i__2].r = diag[i__3].r, diag[i__2].i = diag[i__3].i;
i__2 = i__ - 2;
diag[i__2].r = ctemp.r, diag[i__2].i = ctemp.i;
/* L70: */
}
} else if (ifst > ilst) {
i__1 = ilst;
for (i__ = ifst - 1; i__ >= i__1; --i__) {
i__2 = i__;
ctemp.r = diag[i__2].r, ctemp.i = diag[i__2].i;
i__2 = i__;
i__3 = i__ - 1;
diag[i__2].r = diag[i__3].r, diag[i__2].i = diag[i__3].i;
i__2 = i__ - 1;
diag[i__2].r = ctemp.r, diag[i__2].i = ctemp.i;
/* L80: */
}
}
i__1 = n;
for (i__ = 1; i__ <= i__1; ++i__) {
i__2 = t2_subscr(i__, i__);
i__3 = i__ - 1;
if (t2[i__2].r != diag[i__3].r || t2[i__2].i != diag[i__3].i) {
res += 1. / eps;
}
/* L90: */
}
/* Test for small residual, and orthogonality of Q */
zhst01_(&n, &c__1, &n, tmp, &c__10, t2, &c__10, q, &c__10, work, &c__200,
rwork, result);
res = res + result[0] + result[1];
/* Test for T2 being in Schur form */
i__1 = n - 1;
for (j = 1; j <= i__1; ++j) {
i__2 = n;
for (i__ = j + 1; i__ <= i__2; ++i__) {
i__3 = t2_subscr(i__, j);
if (t2[i__3].r != 0. || t2[i__3].i != 0.) {
res += 1. / eps;
}
/* L100: */
}
/* L110: */
}
if (res > *rmax) {
*rmax = res;
*lmax = *knt;
}
goto L10;
/* End of ZGET36 */
} /* zget36_ */
scl::sBool CExampleApp::setInitialStateForUIAndDynamics()
{
bool flag;
try
{
// NOTE TODO : Vinay
//Get joint angles a few times here.... Set them in the robot io data structure
robot_.getData()->io_data_->sensors_.q_; // Set me
//Compute dynamics and servo once to initialize matrices.
robot_.computeDynamics();
robot_.computeNonControlOperations();
robot_.computeServo();
robot_.setGeneralizedCoordinatesToZero();
robot_.setGeneralizedVelocitiesToZero();
robot_.setGeneralizedAccelerationsToZero();
robot_.computeDynamics();
robot_.computeNonControlOperations();
robot_.computeServo();
//Update the operational point tasks (if any)
std::vector<SUiCtrlPointData>::iterator it,ite;
for(it = taskvec_ui_ctrl_point_.begin(), ite = taskvec_ui_ctrl_point_.end(); it!=ite; ++it )
{
if(false == it->has_been_init_)
{
throw(std::runtime_error(std::string("UI Task not intialized: ")+it->name_));
}
if(NULL==it->chai_pos_des_)
{
throw(std::runtime_error(std::string("UI Task's chai position vector is NULL: ")+it->name_));
}
if(NULL==it->task_)
{
throw(std::runtime_error(std::string("UI Task's control object is null: ")+it->name_));
}
it->task_->getPos(it->pos_);
flag = (3 == it->pos_.rows() && 1 == it->pos_.cols()) ||
(1 == it->pos_.rows() && 3 == it->pos_.cols());
if( false == flag )
{
throw(std::runtime_error(std::string("UI task's control position vector size is incorrect: ")+it->name_));
}
db_->s_gui_.ui_point_[it->ui_pt_] = it->pos_;
//Using a tmp ref to simplify code.
Eigen::Vector3d& tmp_ref = db_->s_gui_.ui_point_[it->ui_pt_];
it->chai_pos_des_->setLocalPos(tmp_ref(0),tmp_ref(1),tmp_ref(2));
}
{
//Update the operational point tasks (if any)
std::vector<SUiCtrlPointData>::iterator it,ite;
static Eigen::VectorXd tmp_vec;
for(it = taskvec_ui_ctrl_point_.begin(), ite = taskvec_ui_ctrl_point_.end(); it!=ite; ++it )
{
it->task_->getPos(tmp_vec);
db_->s_gui_.ui_point_[it->ui_pt_] << tmp_vec(0),tmp_vec(1),tmp_vec(2);
} //Get the goal position.
}
return true;
}
catch(std::exception &e)
{
std::cerr<<"\nCExampleApp::setInitialStateForUIAndDynamics() : "<<e.what();
}
return false;
}
/* Subroutine */ int dget37_(doublereal *rmax, integer *lmax, integer *ninfo,
integer *knt, integer *nin)
{
/* System generated locals */
integer i__1, i__2;
doublereal d__1, d__2;
/* Builtin functions */
double sqrt(doublereal);
integer s_rsle(cilist *), do_lio(integer *, integer *, char *, ftnlen),
e_rsle(void);
/* Local variables */
static integer ifnd, icmp, iscl, info, lcmp[3], kmin;
static doublereal wiin[20], vmax, tnrm, wrin[20], work[1200], vmul, stmp[
20];
static integer i__, j, m, n;
static doublereal s[20], t[400] /* was [20][20] */, v;
extern /* Subroutine */ int dscal_(integer *, doublereal *, doublereal *,
integer *);
static doublereal sepin[20];
extern /* Subroutine */ int dcopy_(integer *, doublereal *, integer *,
doublereal *, integer *);
static doublereal vimin, tolin, vrmin;
static integer iwork[40];
static doublereal witmp[20], wrtmp[20];
extern /* Subroutine */ int dlabad_(doublereal *, doublereal *);
static doublereal le[400] /* was [20][20] */, re[400] /* was [20][
20] */;
extern doublereal dlamch_(char *), dlange_(char *, integer *,
integer *, doublereal *, integer *, doublereal *);
extern /* Subroutine */ int dgehrd_(integer *, integer *, integer *,
doublereal *, integer *, doublereal *, doublereal *, integer *,
integer *);
static doublereal wi[20], wr[20];
extern /* Subroutine */ int dlacpy_(char *, integer *, integer *,
doublereal *, integer *, doublereal *, integer *);
static logical select[20];
static doublereal bignum;
extern /* Subroutine */ int dhseqr_(char *, char *, integer *, integer *,
integer *, doublereal *, integer *, doublereal *, doublereal *,
doublereal *, integer *, doublereal *, integer *, integer *), dtrevc_(char *, char *, logical *, integer *,
doublereal *, integer *, doublereal *, integer *, doublereal *,
integer *, integer *, integer *, doublereal *, integer *), dtrsna_(char *, char *, logical *, integer *, doublereal
*, integer *, doublereal *, integer *, doublereal *, integer *,
doublereal *, doublereal *, integer *, integer *, doublereal *,
integer *, integer *, integer *);
static doublereal septmp[20], smlnum, val[3], dum[1], eps, sep[20], sin__[
20], tol, tmp[400] /* was [20][20] */;
/* Fortran I/O blocks */
static cilist io___5 = { 0, 0, 0, 0, 0 };
static cilist io___8 = { 0, 0, 0, 0, 0 };
static cilist io___11 = { 0, 0, 0, 0, 0 };
#define t_ref(a_1,a_2) t[(a_2)*20 + a_1 - 21]
#define le_ref(a_1,a_2) le[(a_2)*20 + a_1 - 21]
#define re_ref(a_1,a_2) re[(a_2)*20 + a_1 - 21]
#define tmp_ref(a_1,a_2) tmp[(a_2)*20 + a_1 - 21]
/* -- LAPACK test 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
=======
DGET37 tests DTRSNA, a routine for estimating condition numbers of
eigenvalues and/or right eigenvectors of a matrix.
The test matrices are read from a file with logical unit number NIN.
Arguments
==========
RMAX (output) DOUBLE PRECISION array, dimension (3)
Value of the largest test ratio.
RMAX(1) = largest ratio comparing different calls to DTRSNA
RMAX(2) = largest error in reciprocal condition
numbers taking their conditioning into account
RMAX(3) = largest error in reciprocal condition
numbers not taking their conditioning into
account (may be larger than RMAX(2))
LMAX (output) INTEGER array, dimension (3)
LMAX(i) is example number where largest test ratio
RMAX(i) is achieved. Also:
If DGEHRD returns INFO nonzero on example i, LMAX(1)=i
If DHSEQR returns INFO nonzero on example i, LMAX(2)=i
If DTRSNA returns INFO nonzero on example i, LMAX(3)=i
NINFO (output) INTEGER array, dimension (3)
NINFO(1) = No. of times DGEHRD returned INFO nonzero
NINFO(2) = No. of times DHSEQR returned INFO nonzero
NINFO(3) = No. of times DTRSNA returned INFO nonzero
KNT (output) INTEGER
Total number of examples tested.
NIN (input) INTEGER
Input logical unit number
=====================================================================
Parameter adjustments */
--ninfo;
--lmax;
--rmax;
/* Function Body */
eps = dlamch_("P");
smlnum = dlamch_("S") / eps;
bignum = 1. / smlnum;
dlabad_(&smlnum, &bignum);
/* EPSIN = 2**(-24) = precision to which input data computed */
eps = max(eps,5.9605e-8);
rmax[1] = 0.;
rmax[2] = 0.;
rmax[3] = 0.;
lmax[1] = 0;
lmax[2] = 0;
lmax[3] = 0;
*knt = 0;
ninfo[1] = 0;
ninfo[2] = 0;
ninfo[3] = 0;
val[0] = sqrt(smlnum);
val[1] = 1.;
val[2] = sqrt(bignum);
/* Read input data until N=0. Assume input eigenvalues are sorted
lexicographically (increasing by real part, then decreasing by
imaginary part) */
L10:
io___5.ciunit = *nin;
s_rsle(&io___5);
do_lio(&c__3, &c__1, (char *)&n, (ftnlen)sizeof(integer));
e_rsle();
if (n == 0) {
return 0;
}
i__1 = n;
for (i__ = 1; i__ <= i__1; ++i__) {
io___8.ciunit = *nin;
s_rsle(&io___8);
i__2 = n;
for (j = 1; j <= i__2; ++j) {
do_lio(&c__5, &c__1, (char *)&tmp_ref(i__, j), (ftnlen)sizeof(
doublereal));
}
e_rsle();
/* L20: */
}
i__1 = n;
for (i__ = 1; i__ <= i__1; ++i__) {
io___11.ciunit = *nin;
s_rsle(&io___11);
do_lio(&c__5, &c__1, (char *)&wrin[i__ - 1], (ftnlen)sizeof(
doublereal));
do_lio(&c__5, &c__1, (char *)&wiin[i__ - 1], (ftnlen)sizeof(
doublereal));
do_lio(&c__5, &c__1, (char *)&sin__[i__ - 1], (ftnlen)sizeof(
doublereal));
do_lio(&c__5, &c__1, (char *)&sepin[i__ - 1], (ftnlen)sizeof(
doublereal));
e_rsle();
/* L30: */
}
tnrm = dlange_("M", &n, &n, tmp, &c__20, work);
/* Begin test */
for (iscl = 1; iscl <= 3; ++iscl) {
/* Scale input matrix */
++(*knt);
dlacpy_("F", &n, &n, tmp, &c__20, t, &c__20);
vmul = val[iscl - 1];
i__1 = n;
for (i__ = 1; i__ <= i__1; ++i__) {
dscal_(&n, &vmul, &t_ref(1, i__), &c__1);
/* L40: */
}
if (tnrm == 0.) {
vmul = 1.;
}
/* Compute eigenvalues and eigenvectors */
i__1 = 1200 - n;
dgehrd_(&n, &c__1, &n, t, &c__20, work, &work[n], &i__1, &info);
if (info != 0) {
lmax[1] = *knt;
++ninfo[1];
goto L240;
}
i__1 = n - 2;
for (j = 1; j <= i__1; ++j) {
i__2 = n;
for (i__ = j + 2; i__ <= i__2; ++i__) {
t_ref(i__, j) = 0.;
/* L50: */
}
/* L60: */
}
/* Compute Schur form */
dhseqr_("S", "N", &n, &c__1, &n, t, &c__20, wr, wi, dum, &c__1, work,
&c__1200, &info);
if (info != 0) {
lmax[2] = *knt;
++ninfo[2];
goto L240;
}
/* Compute eigenvectors */
dtrevc_("Both", "All", select, &n, t, &c__20, le, &c__20, re, &c__20,
&n, &m, work, &info);
/* Compute condition numbers */
dtrsna_("Both", "All", select, &n, t, &c__20, le, &c__20, re, &c__20,
s, sep, &n, &m, work, &n, iwork, &info);
if (info != 0) {
lmax[3] = *knt;
++ninfo[3];
goto L240;
}
/* Sort eigenvalues and condition numbers lexicographically
to compare with inputs */
dcopy_(&n, wr, &c__1, wrtmp, &c__1);
dcopy_(&n, wi, &c__1, witmp, &c__1);
dcopy_(&n, s, &c__1, stmp, &c__1);
dcopy_(&n, sep, &c__1, septmp, &c__1);
d__1 = 1. / vmul;
dscal_(&n, &d__1, septmp, &c__1);
i__1 = n - 1;
for (i__ = 1; i__ <= i__1; ++i__) {
kmin = i__;
vrmin = wrtmp[i__ - 1];
vimin = witmp[i__ - 1];
i__2 = n;
for (j = i__ + 1; j <= i__2; ++j) {
if (wrtmp[j - 1] < vrmin) {
kmin = j;
vrmin = wrtmp[j - 1];
vimin = witmp[j - 1];
}
/* L70: */
}
wrtmp[kmin - 1] = wrtmp[i__ - 1];
witmp[kmin - 1] = witmp[i__ - 1];
wrtmp[i__ - 1] = vrmin;
witmp[i__ - 1] = vimin;
vrmin = stmp[kmin - 1];
stmp[kmin - 1] = stmp[i__ - 1];
stmp[i__ - 1] = vrmin;
vrmin = septmp[kmin - 1];
septmp[kmin - 1] = septmp[i__ - 1];
septmp[i__ - 1] = vrmin;
/* L80: */
}
/* Compare condition numbers for eigenvalues
taking their condition numbers into account
Computing MAX */
d__1 = (doublereal) n * 2. * eps * tnrm;
v = max(d__1,smlnum);
if (tnrm == 0.) {
v = 1.;
}
i__1 = n;
for (i__ = 1; i__ <= i__1; ++i__) {
if (v > septmp[i__ - 1]) {
tol = 1.;
} else {
tol = v / septmp[i__ - 1];
}
if (v > sepin[i__ - 1]) {
tolin = 1.;
} else {
tolin = v / sepin[i__ - 1];
}
/* Computing MAX */
d__1 = tol, d__2 = smlnum / eps;
tol = max(d__1,d__2);
/* Computing MAX */
d__1 = tolin, d__2 = smlnum / eps;
tolin = max(d__1,d__2);
if (eps * (sin__[i__ - 1] - tolin) > stmp[i__ - 1] + tol) {
vmax = 1. / eps;
} else if (sin__[i__ - 1] - tolin > stmp[i__ - 1] + tol) {
vmax = (sin__[i__ - 1] - tolin) / (stmp[i__ - 1] + tol);
} else if (sin__[i__ - 1] + tolin < eps * (stmp[i__ - 1] - tol)) {
vmax = 1. / eps;
} else if (sin__[i__ - 1] + tolin < stmp[i__ - 1] - tol) {
vmax = (stmp[i__ - 1] - tol) / (sin__[i__ - 1] + tolin);
} else {
vmax = 1.;
}
if (vmax > rmax[2]) {
rmax[2] = vmax;
if (ninfo[2] == 0) {
lmax[2] = *knt;
}
}
/* L90: */
}
/* Compare condition numbers for eigenvectors
taking their condition numbers into account */
i__1 = n;
for (i__ = 1; i__ <= i__1; ++i__) {
if (v > septmp[i__ - 1] * stmp[i__ - 1]) {
tol = septmp[i__ - 1];
} else {
tol = v / stmp[i__ - 1];
}
if (v > sepin[i__ - 1] * sin__[i__ - 1]) {
tolin = sepin[i__ - 1];
} else {
tolin = v / sin__[i__ - 1];
}
/* Computing MAX */
d__1 = tol, d__2 = smlnum / eps;
tol = max(d__1,d__2);
/* Computing MAX */
d__1 = tolin, d__2 = smlnum / eps;
tolin = max(d__1,d__2);
if (eps * (sepin[i__ - 1] - tolin) > septmp[i__ - 1] + tol) {
vmax = 1. / eps;
} else if (sepin[i__ - 1] - tolin > septmp[i__ - 1] + tol) {
vmax = (sepin[i__ - 1] - tolin) / (septmp[i__ - 1] + tol);
} else if (sepin[i__ - 1] + tolin < eps * (septmp[i__ - 1] - tol))
{
vmax = 1. / eps;
} else if (sepin[i__ - 1] + tolin < septmp[i__ - 1] - tol) {
vmax = (septmp[i__ - 1] - tol) / (sepin[i__ - 1] + tolin);
} else {
vmax = 1.;
}
if (vmax > rmax[2]) {
rmax[2] = vmax;
if (ninfo[2] == 0) {
lmax[2] = *knt;
}
}
/* L100: */
}
/* Compare condition numbers for eigenvalues
without taking their condition numbers into account */
i__1 = n;
for (i__ = 1; i__ <= i__1; ++i__) {
if (sin__[i__ - 1] <= (doublereal) (n << 1) * eps && stmp[i__ - 1]
<= (doublereal) (n << 1) * eps) {
vmax = 1.;
} else if (eps * sin__[i__ - 1] > stmp[i__ - 1]) {
vmax = 1. / eps;
} else if (sin__[i__ - 1] > stmp[i__ - 1]) {
vmax = sin__[i__ - 1] / stmp[i__ - 1];
} else if (sin__[i__ - 1] < eps * stmp[i__ - 1]) {
vmax = 1. / eps;
} else if (sin__[i__ - 1] < stmp[i__ - 1]) {
vmax = stmp[i__ - 1] / sin__[i__ - 1];
} else {
vmax = 1.;
}
if (vmax > rmax[3]) {
rmax[3] = vmax;
if (ninfo[3] == 0) {
lmax[3] = *knt;
}
}
/* L110: */
}
/* Compare condition numbers for eigenvectors
without taking their condition numbers into account */
i__1 = n;
for (i__ = 1; i__ <= i__1; ++i__) {
if (sepin[i__ - 1] <= v && septmp[i__ - 1] <= v) {
vmax = 1.;
} else if (eps * sepin[i__ - 1] > septmp[i__ - 1]) {
vmax = 1. / eps;
} else if (sepin[i__ - 1] > septmp[i__ - 1]) {
vmax = sepin[i__ - 1] / septmp[i__ - 1];
} else if (sepin[i__ - 1] < eps * septmp[i__ - 1]) {
vmax = 1. / eps;
} else if (sepin[i__ - 1] < septmp[i__ - 1]) {
vmax = septmp[i__ - 1] / sepin[i__ - 1];
} else {
vmax = 1.;
}
if (vmax > rmax[3]) {
rmax[3] = vmax;
if (ninfo[3] == 0) {
lmax[3] = *knt;
}
}
/* L120: */
}
/* Compute eigenvalue condition numbers only and compare */
vmax = 0.;
dum[0] = -1.;
dcopy_(&n, dum, &c__0, stmp, &c__1);
dcopy_(&n, dum, &c__0, septmp, &c__1);
dtrsna_("Eigcond", "All", select, &n, t, &c__20, le, &c__20, re, &
c__20, stmp, septmp, &n, &m, work, &n, iwork, &info);
if (info != 0) {
lmax[3] = *knt;
++ninfo[3];
goto L240;
}
i__1 = n;
for (i__ = 1; i__ <= i__1; ++i__) {
if (stmp[i__ - 1] != s[i__ - 1]) {
vmax = 1. / eps;
}
if (septmp[i__ - 1] != dum[0]) {
vmax = 1. / eps;
}
/* L130: */
}
/* Compute eigenvector condition numbers only and compare */
dcopy_(&n, dum, &c__0, stmp, &c__1);
dcopy_(&n, dum, &c__0, septmp, &c__1);
dtrsna_("Veccond", "All", select, &n, t, &c__20, le, &c__20, re, &
c__20, stmp, septmp, &n, &m, work, &n, iwork, &info);
if (info != 0) {
lmax[3] = *knt;
++ninfo[3];
goto L240;
}
i__1 = n;
for (i__ = 1; i__ <= i__1; ++i__) {
if (stmp[i__ - 1] != dum[0]) {
vmax = 1. / eps;
}
if (septmp[i__ - 1] != sep[i__ - 1]) {
vmax = 1. / eps;
}
/* L140: */
}
/* Compute all condition numbers using SELECT and compare */
i__1 = n;
for (i__ = 1; i__ <= i__1; ++i__) {
select[i__ - 1] = TRUE_;
/* L150: */
}
dcopy_(&n, dum, &c__0, stmp, &c__1);
dcopy_(&n, dum, &c__0, septmp, &c__1);
dtrsna_("Bothcond", "Some", select, &n, t, &c__20, le, &c__20, re, &
c__20, stmp, septmp, &n, &m, work, &n, iwork, &info);
if (info != 0) {
lmax[3] = *knt;
++ninfo[3];
goto L240;
}
i__1 = n;
for (i__ = 1; i__ <= i__1; ++i__) {
if (septmp[i__ - 1] != sep[i__ - 1]) {
vmax = 1. / eps;
}
if (stmp[i__ - 1] != s[i__ - 1]) {
vmax = 1. / eps;
}
/* L160: */
}
/* Compute eigenvalue condition numbers using SELECT and compare */
dcopy_(&n, dum, &c__0, stmp, &c__1);
dcopy_(&n, dum, &c__0, septmp, &c__1);
dtrsna_("Eigcond", "Some", select, &n, t, &c__20, le, &c__20, re, &
c__20, stmp, septmp, &n, &m, work, &n, iwork, &info);
if (info != 0) {
lmax[3] = *knt;
++ninfo[3];
goto L240;
}
i__1 = n;
for (i__ = 1; i__ <= i__1; ++i__) {
if (stmp[i__ - 1] != s[i__ - 1]) {
vmax = 1. / eps;
}
if (septmp[i__ - 1] != dum[0]) {
vmax = 1. / eps;
}
/* L170: */
}
/* Compute eigenvector condition numbers using SELECT and compare */
dcopy_(&n, dum, &c__0, stmp, &c__1);
dcopy_(&n, dum, &c__0, septmp, &c__1);
dtrsna_("Veccond", "Some", select, &n, t, &c__20, le, &c__20, re, &
c__20, stmp, septmp, &n, &m, work, &n, iwork, &info);
if (info != 0) {
lmax[3] = *knt;
++ninfo[3];
goto L240;
}
i__1 = n;
for (i__ = 1; i__ <= i__1; ++i__) {
if (stmp[i__ - 1] != dum[0]) {
vmax = 1. / eps;
}
if (septmp[i__ - 1] != sep[i__ - 1]) {
vmax = 1. / eps;
}
/* L180: */
}
if (vmax > rmax[1]) {
rmax[1] = vmax;
if (ninfo[1] == 0) {
lmax[1] = *knt;
}
}
/* Select first real and first complex eigenvalue */
if (wi[0] == 0.) {
lcmp[0] = 1;
ifnd = 0;
i__1 = n;
for (i__ = 2; i__ <= i__1; ++i__) {
if (ifnd == 1 || wi[i__ - 1] == 0.) {
select[i__ - 1] = FALSE_;
} else {
ifnd = 1;
lcmp[1] = i__;
lcmp[2] = i__ + 1;
dcopy_(&n, &re_ref(1, i__), &c__1, &re_ref(1, 2), &c__1);
dcopy_(&n, &re_ref(1, i__ + 1), &c__1, &re_ref(1, 3), &
c__1);
dcopy_(&n, &le_ref(1, i__), &c__1, &le_ref(1, 2), &c__1);
dcopy_(&n, &le_ref(1, i__ + 1), &c__1, &le_ref(1, 3), &
c__1);
}
/* L190: */
}
if (ifnd == 0) {
icmp = 1;
} else {
icmp = 3;
}
} else {
lcmp[0] = 1;
lcmp[1] = 2;
ifnd = 0;
i__1 = n;
for (i__ = 3; i__ <= i__1; ++i__) {
if (ifnd == 1 || wi[i__ - 1] != 0.) {
select[i__ - 1] = FALSE_;
} else {
lcmp[2] = i__;
ifnd = 1;
dcopy_(&n, &re_ref(1, i__), &c__1, &re_ref(1, 3), &c__1);
dcopy_(&n, &le_ref(1, i__), &c__1, &le_ref(1, 3), &c__1);
}
/* L200: */
}
if (ifnd == 0) {
icmp = 2;
} else {
icmp = 3;
}
}
/* Compute all selected condition numbers */
dcopy_(&icmp, dum, &c__0, stmp, &c__1);
dcopy_(&icmp, dum, &c__0, septmp, &c__1);
dtrsna_("Bothcond", "Some", select, &n, t, &c__20, le, &c__20, re, &
c__20, stmp, septmp, &n, &m, work, &n, iwork, &info);
if (info != 0) {
lmax[3] = *knt;
++ninfo[3];
goto L240;
}
i__1 = icmp;
for (i__ = 1; i__ <= i__1; ++i__) {
j = lcmp[i__ - 1];
if (septmp[i__ - 1] != sep[j - 1]) {
vmax = 1. / eps;
}
if (stmp[i__ - 1] != s[j - 1]) {
vmax = 1. / eps;
}
/* L210: */
}
/* Compute selected eigenvalue condition numbers */
dcopy_(&icmp, dum, &c__0, stmp, &c__1);
dcopy_(&icmp, dum, &c__0, septmp, &c__1);
dtrsna_("Eigcond", "Some", select, &n, t, &c__20, le, &c__20, re, &
c__20, stmp, septmp, &n, &m, work, &n, iwork, &info);
if (info != 0) {
lmax[3] = *knt;
++ninfo[3];
goto L240;
}
i__1 = icmp;
for (i__ = 1; i__ <= i__1; ++i__) {
j = lcmp[i__ - 1];
if (stmp[i__ - 1] != s[j - 1]) {
vmax = 1. / eps;
}
if (septmp[i__ - 1] != dum[0]) {
vmax = 1. / eps;
}
/* L220: */
}
/* Compute selected eigenvector condition numbers */
dcopy_(&icmp, dum, &c__0, stmp, &c__1);
dcopy_(&icmp, dum, &c__0, septmp, &c__1);
dtrsna_("Veccond", "Some", select, &n, t, &c__20, le, &c__20, re, &
c__20, stmp, septmp, &n, &m, work, &n, iwork, &info);
if (info != 0) {
lmax[3] = *knt;
++ninfo[3];
goto L240;
}
i__1 = icmp;
for (i__ = 1; i__ <= i__1; ++i__) {
j = lcmp[i__ - 1];
if (stmp[i__ - 1] != dum[0]) {
vmax = 1. / eps;
}
if (septmp[i__ - 1] != sep[j - 1]) {
vmax = 1. / eps;
}
/* L230: */
}
if (vmax > rmax[1]) {
rmax[1] = vmax;
if (ninfo[1] == 0) {
lmax[1] = *knt;
}
}
L240:
;
}
goto L10;
/* End of DGET37 */
} /* dget37_ */
