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

   1 #include "clapack.h"
   2
   3 /* Subroutine */ int slartg_(real *f, real *g, real *cs, real *sn, real *r__)
   4 {
   5     /* System generated locals */
   6     integer i__1;
   7     real r__1, r__2;
   8
   9     /* Builtin functions */
  10     double log(doublereal), pow_ri(real *, integer *), sqrt(doublereal);
  11
  12     /* Local variables */
  13     integer i__;
  14     real f1, g1, eps, scale;
  15     integer count;
  16     real safmn2, safmx2;
  17     extern doublereal slamch_(char *);
  18     real safmin;
  19
  20
  21 /*  -- LAPACK auxiliary routine (version 3.1) -- */
  22 /*     Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
  23 /*     November 2006 */
  24
  25 /*     .. Scalar Arguments .. */
  26 /*     .. */
  27
  28 /*  Purpose */
  29 /*  ======= */
  30
  31 /*  SLARTG generate a plane rotation so that */
  32
  33 /*     [  CS  SN  ]  .  [ F ]  =  [ R ]   where CS**2 + SN**2 = 1. */
  34 /*     [ -SN  CS  ]     [ G ]     [ 0 ] */
  35
  36 /*  This is a slower, more accurate version of the BLAS1 routine SROTG, */
  37 /*  with the following other differences: */
  38 /*     F and G are unchanged on return. */
  39 /*     If G=0, then CS=1 and SN=0. */
  40 /*     If F=0 and (G .ne. 0), then CS=0 and SN=1 without doing any */
  41 /*        floating point operations (saves work in SBDSQR when */
  42 /*        there are zeros on the diagonal). */
  43
  44 /*  If F exceeds G in magnitude, CS will be positive. */
  45
  46 /*  Arguments */
  47 /*  ========= */
  48
  49 /*  F       (input) REAL */
  50 /*          The first component of vector to be rotated. */
  51
  52 /*  G       (input) REAL */
  53 /*          The second component of vector to be rotated. */
  54
  55 /*  CS      (output) REAL */
  56 /*          The cosine of the rotation. */
  57
  58 /*  SN      (output) REAL */
  59 /*          The sine of the rotation. */
  60
  61 /*  R       (output) REAL */
  62 /*          The nonzero component of the rotated vector. */
  63
  64 /*  This version has a few statements commented out for thread safety */
  65 /*  (machine parameters are computed on each entry). 10 feb 03, SJH. */
  66
  67 /*  ===================================================================== */
  68
  69 /*     .. Parameters .. */
  70 /*     .. */
  71 /*     .. Local Scalars .. */
  72 /*     LOGICAL            FIRST */
  73 /*     .. */
  74 /*     .. External Functions .. */
  75 /*     .. */
  76 /*     .. Intrinsic Functions .. */
  77 /*     .. */
  78 /*     .. Save statement .. */
  79 /*     SAVE               FIRST, SAFMX2, SAFMIN, SAFMN2 */
  80 /*     .. */
  81 /*     .. Data statements .. */
  82 /*     DATA               FIRST / .TRUE. / */
  83 /*     .. */
  84 /*     .. Executable Statements .. */
  85
  86 /*     IF( FIRST ) THEN */
  87     safmin = slamch_("S");
  88     eps = slamch_("E");
  89     r__1 = slamch_("B");
  90     i__1 = (integer) (log(safmin / eps) / log(slamch_("B")) / 2.f);
  91     safmn2 = pow_ri(&r__1, &i__1);
  92     safmx2 = 1.f / safmn2;
  93 /*        FIRST = .FALSE. */
  94 /*     END IF */
  95     if (*g == 0.f) {
  96         *cs = 1.f;
  97         *sn = 0.f;
  98         *r__ = *f;
  99     } else if (*f == 0.f) {
 100         *cs = 0.f;
 101         *sn = 1.f;
 102         *r__ = *g;
 103     } else {
 104         f1 = *f;
 105         g1 = *g;
 106 /* Computing MAX */
 107         r__1 = dabs(f1), r__2 = dabs(g1);
 108         scale = dmax(r__1,r__2);
 109         if (scale >= safmx2) {
 110             count = 0;
 111 L10:
 112             ++count;
 113             f1 *= safmn2;
 114             g1 *= safmn2;
 115 /* Computing MAX */
 116             r__1 = dabs(f1), r__2 = dabs(g1);
 117             scale = dmax(r__1,r__2);
 118             if (scale >= safmx2) {
 119                 goto L10;
 120             }
 121 /* Computing 2nd power */
 122             r__1 = f1;
 123 /* Computing 2nd power */
 124             r__2 = g1;
 125             *r__ = sqrt(r__1 * r__1 + r__2 * r__2);
 126             *cs = f1 / *r__;
 127             *sn = g1 / *r__;
 128             i__1 = count;
 129             for (i__ = 1; i__ <= i__1; ++i__) {
 130                 *r__ *= safmx2;
 131 /* L20: */
 132             }
 133         } else if (scale <= safmn2) {
 134             count = 0;
 135 L30:
 136             ++count;
 137             f1 *= safmx2;
 138             g1 *= safmx2;
 139 /* Computing MAX */
 140             r__1 = dabs(f1), r__2 = dabs(g1);
 141             scale = dmax(r__1,r__2);
 142             if (scale <= safmn2) {
 143                 goto L30;
 144             }
 145 /* Computing 2nd power */
 146             r__1 = f1;
 147 /* Computing 2nd power */
 148             r__2 = g1;
 149             *r__ = sqrt(r__1 * r__1 + r__2 * r__2);
 150             *cs = f1 / *r__;
 151             *sn = g1 / *r__;
 152             i__1 = count;
 153             for (i__ = 1; i__ <= i__1; ++i__) {
 154                 *r__ *= safmn2;
 155 /* L40: */
 156             }
 157         } else {
 158 /* Computing 2nd power */
 159             r__1 = f1;
 160 /* Computing 2nd power */
 161             r__2 = g1;
 162             *r__ = sqrt(r__1 * r__1 + r__2 * r__2);
 163             *cs = f1 / *r__;
 164             *sn = g1 / *r__;
 165         }
 166         if (dabs(*f) > dabs(*g) && *cs < 0.f) {
 167             *cs = -(*cs);
 168             *sn = -(*sn);
 169             *r__ = -(*r__);
 170         }
 171     }
 172     return 0;
 173
 174 /*     End of SLARTG */
 175
 176 } /* slartg_ */