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