home *** CD-ROM | disk | FTP | other *** search
/ Fish 'n' More 2 / fishmore-publicdomainlibraryvol.ii1991xetec.iso / fish / applications / xlispstat / src / src2.lzh / XLisp-Stat / makerotation.c < prev    next >
C/C++ Source or Header  |  1990-10-02  |  2KB  |  72 lines
  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 "xlisp.h"
  8. #include "osdef.h"
  9. #ifdef ANSI
  10. #include "xlsproto.h"
  11. #else
  12. #include "xlsfun.h"
  13. #endif ANSI
  14.  
  15. #ifdef ANSI
  16. double inner_product(int,RVector,RVector);
  17. #else
  18. double inner_product();
  19. #endif ANSI
  20.  
  21. static double inner_product(n, x, y)
  22.     int n;
  23.     RVector x, y;
  24. {
  25.   double result = 0.0;
  26.   
  27.   for (; n > 0; n--, x++, y++) result += *x * *y;
  28.   return(result);
  29. }
  30.   
  31. #define NORM(n, x) (sqrt(inner_product(n, x, x)))
  32.  
  33. void make_rotation(n, rot, x, y, use_alpha, alpha)
  34.     int n, use_alpha;
  35.     RMatrix rot;
  36.     RVector x, y;
  37.     double alpha;
  38. {
  39.   double nx, ny, xy, c, s;
  40.   int i, j;
  41.   
  42.   for (i = 0; i < n; i++) {
  43.     for (j = 0; j < n; j++) rot[i][j] = 0.0;
  44.     rot[i][i] = 1.0;
  45.   }
  46.   
  47.   nx = NORM(n, x);
  48.   ny = NORM(n, y);
  49.   if (nx == 0.0 || ny == 0.0) return;
  50.   
  51.   for (i = 0; i < n; i++) x[i] /= nx;
  52.   for (i = 0; i < n; i++) y[i] /= ny;
  53.   
  54.   xy = inner_product(n, x, y);
  55.   c = (use_alpha) ? cos(alpha) : xy;
  56.   s = (use_alpha) ? sin(alpha) : sqrt(1 - c * c);
  57.   
  58.   for (i = 0; i < n; i++) y[i] -= xy * x[i];
  59.  
  60.   ny = NORM(n, y);
  61.   if (ny == 0.0) return;
  62.   for (i = 0; i < n; i++) y[i] /= ny;
  63.   
  64.   for (i = 0; i < n; i++) {
  65.     for (j = 0; j < n; j++) 
  66.       rot[i][j] = x[j] * (  x[i] * (c - 1.0) + y[i] * s)
  67.                 + y[j] * (- x[i] * s + y[i] * (c - 1.0));
  68.     rot[i][i] += 1.0;
  69.   }
  70. }
  71.  
  72.