File: makerot.c

package info (click to toggle)
xlispstat 3.52.14-1
  • links: PTS
  • area: main
  • in suites: potato
  • size: 7,560 kB
  • ctags: 12,676
  • sloc: ansic: 91,357; lisp: 21,759; sh: 1,525; makefile: 521; csh: 1
file content (54 lines) | stat: -rw-r--r-- 1,403 bytes parent folder | download | duplicates (4)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
/* makerotation - Construct rotation from x to y by alpha.             */
/* XLISP-STAT 2.1 Copyright (c) 1990, by Luke Tierney                  */
/* Additions to Xlisp 2.1, Copyright (c) 1989 by David Michael Betz    */
/* You may give out copies of this software; for conditions see the    */
/* file COPYING included with this distribution.                       */
 
#include "linalg.h"

VOID make_rotation P6C(int, n,
		       double *, rot,
		       double *, x,
		       double *, y,
		       int, use_alpha,
		       double, alpha)
{
  double nx, ny, xy, c, s, cc, cm1, a, b;
  int i, j, in;
  
  for (i = 0, in = 0; i < n; i++, in += n) {
    for (j = 0; j < n; j++)
      rot[in + j] = 0.0;
    rot[in + i] = 1.0;
  }
  
  nx = blas_dnrm2(n, x, 1);
  ny = blas_dnrm2(n, y, 1);
  if (nx == 0.0 || ny == 0.0) return;
  
  blas_dscal(n, 1.0 / nx, x, 1);
  blas_dscal(n, 1.0 / ny, y, 1);

  xy = blas_ddot(n, x, 1, y, 1);

  c = (use_alpha) ? cos(alpha) : xy;
  cc = 1 - c * c;
  s = (use_alpha) ? sin(alpha) : sqrt(cc > 0 ? cc : 0.0);
  cm1 = c - 1.0;

  blas_daxpy(n, -xy, x, 1, y, 1);

  ny = blas_dnrm2(n, y, 1);
  if (ny == 0.0)
    return;
  blas_dscal(n, 1.0 / ny, y, 1);
  
  for (i = 0, in = 0; i < n; i++, in += n) {
    a =   x[i] * cm1 + y[i] * s;
    b = - x[i] * s   + y[i] * cm1;
    for (j = 0; j < n; j++) 
      rot[in + j] = a * x[j] + b * y[j];
    rot[in + i] += 1.0;
  }
}