redoing dev style to be more test centric, from lessons learned with lisp-matrix.
[CommonLispStat.git] / lib / makerotation.c
blob6b7deccd0a4a4bcb50c87fb3c3e4cb62e33cc3c3
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(size_t n, RVector x, RVector y)
11 double result = 0.0;
13 for (; n > 0; n--, x++, y++) result += *x * *y;
14 return(result);
17 #define NORM(n, x) (sqrt(inner_product(n, x, x)))
19 void
20 make_rotation(size_t n, double **rot, double *x, double *y, int use_alpha, double alpha)
22 double nx, ny, xy, c, s;
23 size_t i, j;
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;
30 nx = NORM(n, x);
31 ny = NORM(n, y);
32 if (nx == 0.0 || ny == 0.0) return;
34 for (i = 0; i < n; i++) x[i] /= nx;
35 for (i = 0; i < n; i++) y[i] /= ny;
37 xy = inner_product(n, x, y);
38 c = (use_alpha) ? cos(alpha) : xy;
39 s = (use_alpha) ? sin(alpha) : sqrt(1 - c * c);
41 for (i = 0; i < n; i++) y[i] -= xy * x[i];
43 ny = NORM(n, y);
44 if (ny == 0.0) return;
45 for (i = 0; i < n; i++) y[i] /= ny;
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;