src/c/makerot.c

   1 /* makerotation - Construct rotation from x to y by alpha.             */
   2 /* XLISP-STAT 2.1 Copyright (c) 1990, by Luke Tierney                  */
   3 /* Additions to Xlisp 2.1, Copyright (c) 1989 by David Michael Betz    */
   4 /* You may give out copies of this software; for conditions see the    */
   5 /* file COPYING included with this distribution.                       */
   6
   7 #include "linalg.h"
   8
   9 VOID make_rotation P6C(int, n,
  10                        double *, rot,
  11                        double *, x,
  12                        double *, y,
  13                        int, use_alpha,
  14                        double, alpha)
  15 {
  16   double nx, ny, xy, c, s, cc, cm1, a, b;
  17   int i, j, in;
  18
  19   for (i = 0, in = 0; i < n; i++, in += n) {
  20     for (j = 0; j < n; j++)
  21       rot[in + j] = 0.0;
  22     rot[in + i] = 1.0;
  23   }
  24
  25   nx = blas_dnrm2(n, x, 1);
  26   ny = blas_dnrm2(n, y, 1);
  27   if (nx == 0.0 || ny == 0.0) return;
  28
  29   blas_dscal(n, 1.0 / nx, x, 1);
  30   blas_dscal(n, 1.0 / ny, y, 1);
  31
  32   xy = blas_ddot(n, x, 1, y, 1);
  33
  34   c = (use_alpha) ? cos(alpha) : xy;
  35   cc = 1 - c * c;
  36   s = (use_alpha) ? sin(alpha) : sqrt(cc > 0 ? cc : 0.0);
  37   cm1 = c - 1.0;
  38
  39   blas_daxpy(n, -xy, x, 1, y, 1);
  40
  41   ny = blas_dnrm2(n, y, 1);
  42   if (ny == 0.0)
  43     return;
  44   blas_dscal(n, 1.0 / ny, y, 1);
  45
  46   for (i = 0, in = 0; i < n; i++, in += n) {
  47     a =   x[i] * cm1 + y[i] * s;
  48     b = - x[i] * s   + y[i] * cm1;
  49     for (j = 0; j < n; j++)
  50       rot[in + j] = a * x[j] + b * y[j];
  51     rot[in + i] += 1.0;
  52   }
  53 }
  54