git.maemo.org Git - opencv/blob - 3rdparty/lapack/dlaed6.c

   1 #include "clapack.h"
   2
   3 /* Subroutine */ int dlaed6_(integer *kniter, logical *orgati, doublereal *
   4         rho, doublereal *d__, doublereal *z__, doublereal *finit, doublereal *
   5         tau, integer *info)
   6 {
   7     /* System generated locals */
   8     integer i__1;
   9     doublereal d__1, d__2, d__3, d__4;
  10
  11     /* Builtin functions */
  12     double sqrt(doublereal), log(doublereal), pow_di(doublereal *, integer *);
  13
  14     /* Local variables */
  15     doublereal a, b, c__, f;
  16     integer i__;
  17     doublereal fc, df, ddf, lbd, eta, ubd, eps, base;
  18     integer iter;
  19     doublereal temp, temp1, temp2, temp3, temp4;
  20     logical scale;
  21     integer niter;
  22     doublereal small1, small2, sminv1, sminv2;
  23     extern doublereal dlamch_(char *);
  24     doublereal dscale[3], sclfac, zscale[3], erretm, sclinv;
  25
  26
  27 /*  -- LAPACK routine (version 3.1.1) -- */
  28 /*     Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
  29 /*     February 2007 */
  30
  31 /*     .. Scalar Arguments .. */
  32 /*     .. */
  33 /*     .. Array Arguments .. */
  34 /*     .. */
  35
  36 /*  Purpose */
  37 /*  ======= */
  38
  39 /*  DLAED6 computes the positive or negative root (closest to the origin) */
  40 /*  of */
  41 /*                   z(1)        z(2)        z(3) */
  42 /*  f(x) =   rho + --------- + ---------- + --------- */
  43 /*                  d(1)-x      d(2)-x      d(3)-x */
  44
  45 /*  It is assumed that */
  46
  47 /*        if ORGATI = .true. the root is between d(2) and d(3); */
  48 /*        otherwise it is between d(1) and d(2) */
  49
  50 /*  This routine will be called by DLAED4 when necessary. In most cases, */
  51 /*  the root sought is the smallest in magnitude, though it might not be */
  52 /*  in some extremely rare situations. */
  53
  54 /*  Arguments */
  55 /*  ========= */
  56
  57 /*  KNITER       (input) INTEGER */
  58 /*               Refer to DLAED4 for its significance. */
  59
  60 /*  ORGATI       (input) LOGICAL */
  61 /*               If ORGATI is true, the needed root is between d(2) and */
  62 /*               d(3); otherwise it is between d(1) and d(2).  See */
  63 /*               DLAED4 for further details. */
  64
  65 /*  RHO          (input) DOUBLE PRECISION */
  66 /*               Refer to the equation f(x) above. */
  67
  68 /*  D            (input) DOUBLE PRECISION array, dimension (3) */
  69 /*               D satisfies d(1) < d(2) < d(3). */
  70
  71 /*  Z            (input) DOUBLE PRECISION array, dimension (3) */
  72 /*               Each of the elements in z must be positive. */
  73
  74 /*  FINIT        (input) DOUBLE PRECISION */
  75 /*               The value of f at 0. It is more accurate than the one */
  76 /*               evaluated inside this routine (if someone wants to do */
  77 /*               so). */
  78
  79 /*  TAU          (output) DOUBLE PRECISION */
  80 /*               The root of the equation f(x). */
  81
  82 /*  INFO         (output) INTEGER */
  83 /*               = 0: successful exit */
  84 /*               > 0: if INFO = 1, failure to converge */
  85
  86 /*  Further Details */
  87 /*  =============== */
  88
  89 /*  30/06/99: Based on contributions by */
  90 /*     Ren-Cang Li, Computer Science Division, University of California */
  91 /*     at Berkeley, USA */
  92
  93 /*  10/02/03: This version has a few statements commented out for thread */
  94 /*  safety (machine parameters are computed on each entry). SJH. */
  95
  96 /*  05/10/06: Modified from a new version of Ren-Cang Li, use */
  97 /*     Gragg-Thornton-Warner cubic convergent scheme for better stability. */
  98
  99 /*  ===================================================================== */
 100
 101 /*     .. Parameters .. */
 102 /*     .. */
 103 /*     .. External Functions .. */
 104 /*     .. */
 105 /*     .. Local Arrays .. */
 106 /*     .. */
 107 /*     .. Local Scalars .. */
 108 /*     .. */
 109 /*     .. Intrinsic Functions .. */
 110 /*     .. */
 111 /*     .. Executable Statements .. */
 112
 113     /* Parameter adjustments */
 114     --z__;
 115     --d__;
 116
 117     /* Function Body */
 118     *info = 0;
 119
 120     if (*orgati) {
 121         lbd = d__[2];
 122         ubd = d__[3];
 123     } else {
 124         lbd = d__[1];
 125         ubd = d__[2];
 126     }
 127     if (*finit < 0.) {
 128         lbd = 0.;
 129     } else {
 130         ubd = 0.;
 131     }
 132
 133     niter = 1;
 134     *tau = 0.;
 135     if (*kniter == 2) {
 136         if (*orgati) {
 137             temp = (d__[3] - d__[2]) / 2.;
 138             c__ = *rho + z__[1] / (d__[1] - d__[2] - temp);
 139             a = c__ * (d__[2] + d__[3]) + z__[2] + z__[3];
 140             b = c__ * d__[2] * d__[3] + z__[2] * d__[3] + z__[3] * d__[2];
 141         } else {
 142             temp = (d__[1] - d__[2]) / 2.;
 143             c__ = *rho + z__[3] / (d__[3] - d__[2] - temp);
 144             a = c__ * (d__[1] + d__[2]) + z__[1] + z__[2];
 145             b = c__ * d__[1] * d__[2] + z__[1] * d__[2] + z__[2] * d__[1];
 146         }
 147 /* Computing MAX */
 148         d__1 = abs(a), d__2 = abs(b), d__1 = max(d__1,d__2), d__2 = abs(c__);
 149         temp = max(d__1,d__2);
 150         a /= temp;
 151         b /= temp;
 152         c__ /= temp;
 153         if (c__ == 0.) {
 154             *tau = b / a;
 155         } else if (a <= 0.) {
 156             *tau = (a - sqrt((d__1 = a * a - b * 4. * c__, abs(d__1)))) / (
 157                     c__ * 2.);
 158         } else {
 159             *tau = b * 2. / (a + sqrt((d__1 = a * a - b * 4. * c__, abs(d__1))
 160                     ));
 161         }
 162         if (*tau < lbd || *tau > ubd) {
 163             *tau = (lbd + ubd) / 2.;
 164         }
 165         if (d__[1] == *tau || d__[2] == *tau || d__[3] == *tau) {
 166             *tau = 0.;
 167         } else {
 168             temp = *finit + *tau * z__[1] / (d__[1] * (d__[1] - *tau)) + *tau
 169                     * z__[2] / (d__[2] * (d__[2] - *tau)) + *tau * z__[3] / (
 170                     d__[3] * (d__[3] - *tau));
 171             if (temp <= 0.) {
 172                 lbd = *tau;
 173             } else {
 174                 ubd = *tau;
 175             }
 176             if (abs(*finit) <= abs(temp)) {
 177                 *tau = 0.;
 178             }
 179         }
 180     }
 181
 182 /*     get machine parameters for possible scaling to avoid overflow */
 183
 184 /*     modified by Sven: parameters SMALL1, SMINV1, SMALL2, */
 185 /*     SMINV2, EPS are not SAVEd anymore between one call to the */
 186 /*     others but recomputed at each call */
 187
 188     eps = dlamch_("Epsilon");
 189     base = dlamch_("Base");
 190     i__1 = (integer) (log(dlamch_("SafMin")) / log(base) / 3.);
 191     small1 = pow_di(&base, &i__1);
 192     sminv1 = 1. / small1;
 193     small2 = small1 * small1;
 194     sminv2 = sminv1 * sminv1;
 195
 196 /*     Determine if scaling of inputs necessary to avoid overflow */
 197 /*     when computing 1/TEMP**3 */
 198
 199     if (*orgati) {
 200 /* Computing MIN */
 201         d__3 = (d__1 = d__[2] - *tau, abs(d__1)), d__4 = (d__2 = d__[3] - *
 202                 tau, abs(d__2));
 203         temp = min(d__3,d__4);
 204     } else {
 205 /* Computing MIN */
 206         d__3 = (d__1 = d__[1] - *tau, abs(d__1)), d__4 = (d__2 = d__[2] - *
 207                 tau, abs(d__2));
 208         temp = min(d__3,d__4);
 209     }
 210     scale = FALSE_;
 211     if (temp <= small1) {
 212         scale = TRUE_;
 213         if (temp <= small2) {
 214
 215 /*        Scale up by power of radix nearest 1/SAFMIN**(2/3) */
 216
 217             sclfac = sminv2;
 218             sclinv = small2;
 219         } else {
 220
 221 /*        Scale up by power of radix nearest 1/SAFMIN**(1/3) */
 222
 223             sclfac = sminv1;
 224             sclinv = small1;
 225         }
 226
 227 /*        Scaling up safe because D, Z, TAU scaled elsewhere to be O(1) */
 228
 229         for (i__ = 1; i__ <= 3; ++i__) {
 230             dscale[i__ - 1] = d__[i__] * sclfac;
 231             zscale[i__ - 1] = z__[i__] * sclfac;
 232 /* L10: */
 233         }
 234         *tau *= sclfac;
 235         lbd *= sclfac;
 236         ubd *= sclfac;
 237     } else {
 238
 239 /*        Copy D and Z to DSCALE and ZSCALE */
 240
 241         for (i__ = 1; i__ <= 3; ++i__) {
 242             dscale[i__ - 1] = d__[i__];
 243             zscale[i__ - 1] = z__[i__];
 244 /* L20: */
 245         }
 246     }
 247
 248     fc = 0.;
 249     df = 0.;
 250     ddf = 0.;
 251     for (i__ = 1; i__ <= 3; ++i__) {
 252         temp = 1. / (dscale[i__ - 1] - *tau);
 253         temp1 = zscale[i__ - 1] * temp;
 254         temp2 = temp1 * temp;
 255         temp3 = temp2 * temp;
 256         fc += temp1 / dscale[i__ - 1];
 257         df += temp2;
 258         ddf += temp3;
 259 /* L30: */
 260     }
 261     f = *finit + *tau * fc;
 262
 263     if (abs(f) <= 0.) {
 264         goto L60;
 265     }
 266     if (f <= 0.) {
 267         lbd = *tau;
 268     } else {
 269         ubd = *tau;
 270     }
 271
 272 /*        Iteration begins -- Use Gragg-Thornton-Warner cubic convergent */
 273 /*                            scheme */
 274
 275 /*     It is not hard to see that */
 276
 277 /*           1) Iterations will go up monotonically */
 278 /*              if FINIT < 0; */
 279
 280 /*           2) Iterations will go down monotonically */
 281 /*              if FINIT > 0. */
 282
 283     iter = niter + 1;
 284
 285     for (niter = iter; niter <= 40; ++niter) {
 286
 287         if (*orgati) {
 288             temp1 = dscale[1] - *tau;
 289             temp2 = dscale[2] - *tau;
 290         } else {
 291             temp1 = dscale[0] - *tau;
 292             temp2 = dscale[1] - *tau;
 293         }
 294         a = (temp1 + temp2) * f - temp1 * temp2 * df;
 295         b = temp1 * temp2 * f;
 296         c__ = f - (temp1 + temp2) * df + temp1 * temp2 * ddf;
 297 /* Computing MAX */
 298         d__1 = abs(a), d__2 = abs(b), d__1 = max(d__1,d__2), d__2 = abs(c__);
 299         temp = max(d__1,d__2);
 300         a /= temp;
 301         b /= temp;
 302         c__ /= temp;
 303         if (c__ == 0.) {
 304             eta = b / a;
 305         } else if (a <= 0.) {
 306             eta = (a - sqrt((d__1 = a * a - b * 4. * c__, abs(d__1)))) / (c__
 307                     * 2.);
 308         } else {
 309             eta = b * 2. / (a + sqrt((d__1 = a * a - b * 4. * c__, abs(d__1)))
 310                     );
 311         }
 312         if (f * eta >= 0.) {
 313             eta = -f / df;
 314         }
 315
 316         *tau += eta;
 317         if (*tau < lbd || *tau > ubd) {
 318             *tau = (lbd + ubd) / 2.;
 319         }
 320
 321         fc = 0.;
 322         erretm = 0.;
 323         df = 0.;
 324         ddf = 0.;
 325         for (i__ = 1; i__ <= 3; ++i__) {
 326             temp = 1. / (dscale[i__ - 1] - *tau);
 327             temp1 = zscale[i__ - 1] * temp;
 328             temp2 = temp1 * temp;
 329             temp3 = temp2 * temp;
 330             temp4 = temp1 / dscale[i__ - 1];
 331             fc += temp4;
 332             erretm += abs(temp4);
 333             df += temp2;
 334             ddf += temp3;
 335 /* L40: */
 336         }
 337         f = *finit + *tau * fc;
 338         erretm = (abs(*finit) + abs(*tau) * erretm) * 8. + abs(*tau) * df;
 339         if (abs(f) <= eps * erretm) {
 340             goto L60;
 341         }
 342         if (f <= 0.) {
 343             lbd = *tau;
 344         } else {
 345             ubd = *tau;
 346         }
 347 /* L50: */
 348     }
 349     *info = 1;
 350 L60:
 351
 352 /*     Undo scaling */
 353
 354     if (scale) {
 355         *tau *= sclinv;
 356     }
 357     return 0;
 358
 359 /*     End of DLAED6 */
 360
 361 } /* dlaed6_ */