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