home *** CD-ROM | disk | FTP | other *** search
/ Celestin Apprentice 2 / Apprentice-Release2.iso / Source Code / C++ / Applications / HyperCuber 2.0 / source / Rotations.cp < prev   
Encoding:
Text File  |  1994-03-27  |  9.0 KB  |  268 lines  |  [TEXT/KAHL]
  1. //|~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  2. //| Rotations.cp
  3. //|
  4. //| This file contains code which implements n-dimensional rotations
  5. //|____________________________________________________________________________
  6.  
  7.  
  8. #include <math.h>
  9.  
  10. //========================== Prototypes ============================\\
  11.  
  12. double **CreateRotMatrix(long dim, double *angles);
  13. void BuildRotMatrix(long dim, double *angles, double **matrix_handle);
  14. void UpdateRotMatrix(long i, double old_value, double new_value, long dim, 
  15.                             double **matrix_handle);
  16. void RotateVector(double *vector, double *rotated_vector, long dim, double *matrix);
  17.  
  18.  
  19. //============================= Constants ==============================\\
  20.  
  21. #define    Pi    3.14159265358979323846
  22.  
  23.  
  24. //|~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  25. //| Procedure CreateRotMatrix
  26. //|
  27. //| Purpose: this procedure creates an dim-dimensional rotation matrix
  28. //|
  29. //| Parameters: dim:     the dimension of this rotation
  30. //|             angles:  a matrix containing the angles
  31. //|             returns a handle to the rotation matrix
  32. //|__________________________________________________________________
  33.  
  34. double **CreateRotMatrix(long dim, double *angles)
  35. {
  36.  
  37.     double **matrix_handle =
  38.             (double **) NewHandleClear(sizeof(double)*dim*dim);        //  Allocate space for matrix
  39.  
  40.     BuildRotMatrix(dim, angles, matrix_handle);
  41.  
  42.     return matrix_handle;                    //  return the matrix
  43.  
  44. }    //==== CreateRotMatrix() ====\\
  45.  
  46.  
  47.  
  48. //|~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  49. //| Procedure BuildRotMatrix
  50. //|
  51. //| Purpose: this procedure fills in a dim-dimensional rotation matrix
  52. //|
  53. //| Parameters: dim:           the dimension of this rotation
  54. //|             angles:        a matrix containing the angles
  55. //|             matrix_handle: handle to the matrix to fill in
  56. //|__________________________________________________________________
  57.  
  58. void BuildRotMatrix(long dim, double *angles, double **matrix_handle)
  59. {
  60.  
  61. //
  62. //  Here is the algorithm used to build the matrix from scratch.  This algorith was
  63. //  derived by examining correct rotation matrices up to size 7x7 (as derived by
  64. //  Mathematica) and looking for patterns.  The patterns were very obvious, but since
  65. //  the analysis was not mathematical, this algorithm has not yet been formally PROVEN.
  66. //  See Mathematica notebook RotMatrix Derivation.
  67. //  
  68. //  M[i,j] is the (i,j)th element of the rotation matrix, and rows and columns both
  69. //  start at 0.  s[i] and c[i] are the ith
  70. //  element of the sines and cosines matrices, respectively.  s[0] is defined to be 1.0
  71. //  (important for proper execution of step 5).
  72. //
  73. //  1. prod = 1; loop c from dim-1 to 1 step -1 { M[0,c] = -prod*s[c-1]; prod *= c[c-1]; }
  74. //  2. M[0, 0] = prod;
  75. //  3. loop r from 1 to dim-1 { M[r, r] = c[r-1] }
  76. //  4. loop r from 1 to dim-2 { loop c from r+1 to dim-1 { M[r, c] = 0 } }
  77. //  5. loop c from 0 to dim-2 { if (c==0) then prod = 1 else prod = -s[c-1];
  78. //                              loop r from c+1 to dim-1 { M[r, c] = prod*s[r-1]; prod *= c[r-1] } }
  79. //
  80. //  The matrix below is a sample 7x7 rotation matrix.  The entries indicate which element
  81. //  is generated by which step of the above algorithm.  For instance, element (0,0) is 2,
  82. //  indicating that the element in the 0th row and 0th column is generated by step 2 of
  83. //  the algorithm.
  84. //
  85. // column = 0           column = dim-1
  86. //     |                       |
  87. //
  88. //  [  2   1   1   1   1   1   1  ]   row = 0
  89. //  [  5   3   4   4   4   4   4  ]
  90. //  [  5   5   3   4   4   4   4  ]
  91. //  [  5   5   5   3   4   4   4  ]
  92. //  [  5   5   5   5   3   4   4  ]
  93. //  [  5   5   5   5   5   3   4  ]
  94. //  [  5   5   5   5   5   0   3  ]   row = dim-1
  95. //
  96.  
  97. #define M(I, J)            *((*matrix_handle) + (I)*dim + (J))
  98. #define ANGLE_OFFSET    Pi/2000
  99.  
  100.     double prod = 1.0;
  101.     long r, c;
  102.     for (c = dim-1; c >= 1; c--)            //  STEP 1
  103.         {
  104.         M(0, c) = -prod*sin(angles[c-1] + ANGLE_OFFSET);
  105.         prod *= cos(angles[c-1] + ANGLE_OFFSET);
  106.         }
  107.     
  108.     M(0, 0) = prod;                            //  STEP 2
  109.  
  110.     for (r = 1; r <= dim-1; r++)            //  STEP 3
  111.         M(r, r) = cos(angles[r-1] + ANGLE_OFFSET);
  112.  
  113.     for (r = 1; r <= dim-2; r++)            //  STEP 4
  114.         for (c = r+1; c <= dim-1; c++)
  115.             M(r, c) = 0;
  116.     
  117.     for (c = 0; c <= dim-2; c++)            //  STEP 5
  118.         {
  119.         if (c == 0)
  120.             prod = 1.0;
  121.         else
  122.             prod = -sin(angles[c-1] + ANGLE_OFFSET);
  123.         for (r = c+1; r<= dim-1; r++)
  124.             {
  125.             M(r, c) = prod * sin(angles[r-1] + ANGLE_OFFSET);
  126.             prod *= cos(angles[r-1] + ANGLE_OFFSET);
  127.             }
  128.         }
  129.  
  130. }    //==== BuildRotMatrix() ====\\
  131.  
  132.  
  133.  
  134. //|~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  135. //| Procedure UpdateRotMatrix
  136. //|
  137. //| Purpose: this procedure updates the rotation matrix when one of the
  138. //|          rotation angles has changed.  Only those elements of the
  139. //|          matrix which depend on that angle are updated.
  140. //|
  141. //| Parameters: i:         the number of the angle which has changed
  142. //|             old_value: the previous value of the angle
  143. //|             new_value: the new value of the angle
  144. //|             matrix_handle: handle to the matrix
  145. //|____________________________________________________________________
  146.  
  147. void UpdateRotMatrix(long i, double old_value, double new_value, long dim, 
  148.                             double **matrix_handle)
  149. {
  150.  
  151. //
  152. //  Here is the algorithm used to update the matrix after one angle had changed.
  153. //  This algorith was derived by examining correct rotation matrices up to size 7x7
  154. //  (as derived by Mathematica) and looking for patterns.  The patterns were very
  155. //  obvious, but since the analysis was not mathematical, this algorithm has not
  156. //  yet been formally PROVEN.  See Mathematica notebook RotMatrix Derivation.
  157. //  
  158. //  Each element of the matrix M is a product of sines and cosines if the angles.
  159. //  No angle appears more than once in a product.  It is therefore possible to
  160. //  quickly update the matrix by multiplying the proper elements by crat and srat,
  161. //  computed in steps 1 and 2, which cancel out the previous value of the sin or
  162. //  cos, and multiply in the new value.
  163. //
  164. //  M[i,j] is the (i,j)th element of the rotation matrix, and rows and columns both
  165. //  start at 0.  i is the angle number.
  166. //
  167. //  1.  crat = cos(new_value)/cos(old_value)
  168. //  2.  srat = sin(new_value)/cos(old_value)
  169. //  3.  M[0, i+1] *= srat
  170. //  4.  loop c from 0 to i { M[0, c] *= crat }
  171. //  5.  M[i+1, i+1] = cos(new_value)
  172. //  6.  loop r from i+2 to dim-1 { loop c from 0 to i { M[r, c] *= crat } }
  173. //  7.  loop c from 0 to i { M[i+1, c] *= srat }
  174. //  8.  loop r from i+2 to dim-1 { M[r, i+1] *= srat }
  175. //
  176. //  The matrix below is a sample 7x7 rotation matrix, indicating how the matrix would be
  177. //  altered when angle 2 was altered.  The entries indicate which element
  178. //  is changed by which step of the above algorithm.  For instance, element (3,3) is 5,
  179. //  indicating that the element in the 3rd row and 3rd column is altered by step 5 of
  180. //  the algorithm.  Elements which are 0 are not altered.
  181. //
  182. //
  183. // column = 0           column = dim-1
  184. //     |                       |
  185. //
  186. //  [  4   4   4   3   0   0   0  ]   row = 0
  187. //  [  0   0   0   0   0   0   0  ]
  188. //  [  0   0   0   0   0   0   0  ]
  189. //  [  7   7   7   5   0   0   0  ]
  190. //  [  6   6   6   8   0   0   0  ]
  191. //  [  6   6   6   8   0   0   0  ]
  192. //  [  6   6   6   8   0   0   0  ]   row = dim-1
  193. //
  194.  
  195.  
  196.     double crat = cos(new_value + ANGLE_OFFSET) / cos(old_value + ANGLE_OFFSET);    //  Step 1
  197.     double srat = sin(new_value + ANGLE_OFFSET) / sin(old_value + ANGLE_OFFSET);    //  Step 2
  198.     long c;
  199.     long r;
  200.     
  201.     M(0, i+1) *= srat;                                //  Step 3
  202.  
  203.     for (c = 0; c <= i; c++)                        //  Step 4
  204.         {
  205.         M(0, c) *= crat;
  206.         }
  207.         
  208.     M(i+1, i+1) = cos(new_value + ANGLE_OFFSET);    //  Step 5
  209.     
  210.     for (r = i+2; r <= dim-1; r++)                    //  Step 6
  211.         {
  212.         for (c = 0; c <= i; c++)
  213.             {
  214.             M(r, c) *= crat;
  215.             }
  216.         }
  217.     
  218.     for (c = 0; c <= i; c++)                        //  Step 7
  219.         {
  220.         M(i+1, c) *= srat;
  221.         }
  222.  
  223.     for (r = i+2; r <= dim-1; r++)                    //  Step 8
  224.         {
  225.         M(r, i+1) *= srat;
  226.         }
  227.  
  228. }    //==== UpdateRotMatrix() ====\\
  229.  
  230.  
  231.  
  232. //|~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  233. //| Procedure RotateVector
  234. //|
  235. //| Purpose: this procedure rotates a vector by multiplying it by
  236. //|          a rotation matrix.
  237. //|
  238. //| Parameters: vector:         the vector to rotate
  239. //|             rotated_vector: receives the rotated vector
  240. //|             dim:            dimension we're rotating in
  241. //|             matrix:         the rotation matrix
  242. //|____________________________________________________________________
  243.  
  244. void RotateVector(double *vector, double *rotated_vector, long dim, double *matrix)
  245. {
  246.     double *matrix_entry = matrix;                    //  Start at first entry in matrix
  247.     double *rot_coordinate = rotated_vector;        //  Start at first coordinate of rotated vect
  248.     
  249.     long row;                                        //  Multiply this vector by the rotation matrix 
  250.     for (row = 1; row <= dim; row++)
  251.         {
  252.         double *coordinate = vector;                //  Start at first coordinate of vector
  253.  
  254.         *rot_coordinate = 0;                        //  Initialize sum to 0
  255.  
  256.         long column;
  257.         for (column = 1; column <= dim; column++)
  258.             {
  259.             *rot_coordinate += (*matrix_entry) * (*coordinate);
  260.             matrix_entry++;
  261.             coordinate++;
  262.             }
  263.         
  264.         rot_coordinate++;
  265.         
  266.         }
  267.         
  268. }    //==== RotateVector() ====\\