lib/makerotation.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 static double inner_product(n, x, y)
  10         int n;
  11         RVector x, y;
  12 {
  13   double result = 0.0;
  14
  15   for (; n > 0; n--, x++, y++) result += *x * *y;
  16   return(result);
  17 }
  18
  19 #define NORM(n, x) (sqrt(inner_product(n, x, x)))
  20
  21 make_rotation(n, rot, x, y, use_alpha, alpha)
  22         int n, use_alpha;
  23         RMatrix rot;
  24         RVector x, y;
  25         double alpha;
  26 {
  27   double nx, ny, xy, c, s;
  28   int i, j;
  29
  30   for (i = 0; i < n; i++) {
  31     for (j = 0; j < n; j++) rot[i][j] = 0.0;
  32     rot[i][i] = 1.0;
  33   }
  34
  35   nx = NORM(n, x);
  36   ny = NORM(n, y);
  37   if (nx == 0.0 || ny == 0.0) return;
  38
  39   for (i = 0; i < n; i++) x[i] /= nx;
  40   for (i = 0; i < n; i++) y[i] /= ny;
  41
  42   xy = inner_product(n, x, y);
  43   c = (use_alpha) ? cos(alpha) : xy;
  44   s = (use_alpha) ? sin(alpha) : sqrt(1 - c * c);
  45
  46   for (i = 0; i < n; i++) y[i] -= xy * x[i];
  47
  48   ny = NORM(n, y);
  49   if (ny == 0.0) return;
  50   for (i = 0; i < n; i++) y[i] /= ny;
  51
  52   for (i = 0; i < n; i++) {
  53     for (j = 0; j < n; j++)
  54       rot[i][j] = x[j] * (  x[i] * (c - 1.0) + y[i] * s)
  55                 + y[j] * (- x[i] * s + y[i] * (c - 1.0));
  56     rot[i][i] += 1.0;
  57   }
  58 }
  59