use doc'd parameter as per SBCL docs; clean up do-indentation; fix ffix macro to...
[CommonLispStat.git] / lib / makerotation.c
blob1e665be1d5d54dd599f9a7cdeed6f2df89f86fcc
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. */
7 #include "linalg.h"
9 static double inner_product(n, x, y)
10 int n;
11 RVector x, y;
13 double result = 0.0;
15 for (; n > 0; n--, x++, y++) result += *x * *y;
16 return(result);
19 #define NORM(n, x) (sqrt(inner_product(n, x, x)))
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;
27 double nx, ny, xy, c, s;
28 int i, j;
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;
35 nx = NORM(n, x);
36 ny = NORM(n, y);
37 if (nx == 0.0 || ny == 0.0) return;
39 for (i = 0; i < n; i++) x[i] /= nx;
40 for (i = 0; i < n; i++) y[i] /= ny;
42 xy = inner_product(n, x, y);
43 c = (use_alpha) ? cos(alpha) : xy;
44 s = (use_alpha) ? sin(alpha) : sqrt(1 - c * c);
46 for (i = 0; i < n; i++) y[i] -= xy * x[i];
48 ny = NORM(n, y);
49 if (ny == 0.0) return;
50 for (i = 0; i < n; i++) y[i] /= ny;
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;