1 | /* nrframe.c ************************************************************** |
---|
2 | |
---|
3 | NR Library Package - Homogeneous Transformation Frames |
---|
4 | |
---|
5 | James Trevelyan, University of Western Australia |
---|
6 | Revision 2 January 1996 |
---|
7 | |
---|
8 | **************************************************************************/ |
---|
9 | |
---|
10 | /* define V_CHECK to include validity checks on matrices */ |
---|
11 | #define V_CHECK |
---|
12 | |
---|
13 | #include <stdio.h> |
---|
14 | #include <math.h> |
---|
15 | #include <stdlib.h> |
---|
16 | #include <string.h> |
---|
17 | #include "nrlin.h" |
---|
18 | |
---|
19 | #define True 1 |
---|
20 | #define False 0 |
---|
21 | #define X 1 |
---|
22 | #define Y 2 |
---|
23 | #define Z 3 |
---|
24 | #define U 4 |
---|
25 | |
---|
26 | #ifdef TEST |
---|
27 | extern FILE *outfile; |
---|
28 | extern double **F1; |
---|
29 | #endif |
---|
30 | |
---|
31 | double **SCR; |
---|
32 | |
---|
33 | double **dframe( void ) |
---|
34 | { |
---|
35 | return ( dmatrix( 1, 4, 1, 4 ) ); |
---|
36 | } |
---|
37 | |
---|
38 | double *d4vector( void ) |
---|
39 | { |
---|
40 | return ( dvector( 1, 4 ) ); |
---|
41 | } |
---|
42 | |
---|
43 | void chainmult( double **a, double **b ) |
---|
44 | /* multiply 4*4 matrices a, b into a, using SCR scratch matrix above */ |
---|
45 | { |
---|
46 | static int once = 0; |
---|
47 | |
---|
48 | if ( !once ) { |
---|
49 | SCR = dmatrix( 1,4, 1,4 ); |
---|
50 | once = True; |
---|
51 | } |
---|
52 | #ifdef V_CHECK |
---|
53 | if ( !valid_dframe( a ) ) |
---|
54 | nrerror("Invalid 1st matrix: chainmult\n"); |
---|
55 | if ( !valid_dframe( b ) ) |
---|
56 | nrerror("Invalid 2nd matrix: chainmult\n"); |
---|
57 | #endif |
---|
58 | dmmult( a, 4, 4, b, 4, 4, SCR ); |
---|
59 | dmcopy( SCR, 4, 4, a ); |
---|
60 | } |
---|
61 | |
---|
62 | |
---|
63 | void rotframe( double **a, int axis, double theta) /* make a rotation transform */ |
---|
64 | { |
---|
65 | double ca, sa; |
---|
66 | int i,j; |
---|
67 | |
---|
68 | #ifdef V_CHECK |
---|
69 | if ( !valid_dframe( a ) ) |
---|
70 | nrerror("Invalid matrix: rotframe\n"); |
---|
71 | #endif |
---|
72 | ca = cos(theta); |
---|
73 | sa = sin(theta); |
---|
74 | for (i=1; i<=4; i++) |
---|
75 | for (j=1; j<=4; j++) a[i][j] = 0.0; |
---|
76 | switch( axis) { |
---|
77 | case 3: |
---|
78 | a[4][4] = a[3][3] = 1.0; |
---|
79 | a[2][2] = a[1][1] = ca; |
---|
80 | a[1][2] = -sa; |
---|
81 | a[2][1] = sa; |
---|
82 | break; |
---|
83 | case 1: |
---|
84 | a[4][4] = a[1][1] = 1.0; |
---|
85 | a[2][2] = a[3][3] = ca; |
---|
86 | a[2][3] = -sa; |
---|
87 | a[3][2] = sa; |
---|
88 | break; |
---|
89 | case 2: |
---|
90 | a[4][4] = a[2][2] = 1.0; |
---|
91 | a[1][1] = a[3][3] = ca; |
---|
92 | a[3][1] = -sa; |
---|
93 | a[1][3] = sa; |
---|
94 | break; |
---|
95 | default: |
---|
96 | fprintf(stderr, "rotframe: axis not X, Y or Z\n"); |
---|
97 | exit(1); |
---|
98 | break; |
---|
99 | } |
---|
100 | } |
---|
101 | |
---|
102 | void transframe( double **a, double dx, double dy, double dz) |
---|
103 | /* make a translation tr'm */ |
---|
104 | { |
---|
105 | int i,j; |
---|
106 | |
---|
107 | #ifdef V_CHECK |
---|
108 | if ( !valid_dframe( a ) ) |
---|
109 | nrerror("Invalid matrix: transframe\n"); |
---|
110 | #endif |
---|
111 | for (i=1; i<=4; i++) |
---|
112 | for (j=1; j<=4; j++) a[i][j] = 0.0; |
---|
113 | a[4][4] = a[3][3] = 1.0; |
---|
114 | a[2][2] = a[1][1] = 1.0; |
---|
115 | a[1][4] = dx; |
---|
116 | a[2][4] = dy; |
---|
117 | a[3][4] = dz; |
---|
118 | } |
---|
119 | |
---|
120 | |
---|
121 | void orthog( double **a ) /* orthogonalize a frame */ |
---|
122 | { |
---|
123 | int i, j, k; |
---|
124 | double s; |
---|
125 | |
---|
126 | #ifdef V_CHECK |
---|
127 | if ( !valid_dframe( a ) ) |
---|
128 | nrerror("Invalid matrix: orthog\n"); |
---|
129 | #endif |
---|
130 | /* cross j, k to produce i */ |
---|
131 | a[1][1] = a[2][2]*a[3][3] - a[3][2]*a[2][3]; |
---|
132 | a[2][1] = a[3][2]*a[1][3] - a[1][2]*a[3][3]; |
---|
133 | a[3][1] = a[1][2]*a[2][3] - a[2][2]*a[1][3]; |
---|
134 | |
---|
135 | /* normalize i, k */ |
---|
136 | s = sqrt(DSQR(a[1][3]) + DSQR(a[2][3]) + DSQR(a[3][3])); |
---|
137 | for (i=1; i<=3; i++) a[i][3] /= s; |
---|
138 | s = sqrt(DSQR(a[1][1]) + DSQR(a[2][1]) + DSQR(a[3][1])); |
---|
139 | for (i=1; i<=3; i++) a[i][1] /= s; |
---|
140 | |
---|
141 | /* cross k, i to produce j */ |
---|
142 | a[1][2] = a[2][3]*a[3][1] - a[3][3]*a[2][1]; |
---|
143 | a[2][2] = a[3][3]*a[1][1] - a[1][3]*a[3][1]; |
---|
144 | a[3][2] = a[1][3]*a[2][1] - a[2][3]*a[1][1]; |
---|
145 | |
---|
146 | } |
---|
147 | |
---|
148 | void orthogKI( double **a ) /* orthogonalize a frame using K & I vector */ |
---|
149 | { |
---|
150 | int i, j, k; |
---|
151 | double s; |
---|
152 | |
---|
153 | #ifdef V_CHECK |
---|
154 | if ( !valid_dframe( a ) ) |
---|
155 | nrerror("Invalid matrix: orthog\n"); |
---|
156 | #endif |
---|
157 | /* cross k, i to produce j */ |
---|
158 | a[1][2] = a[2][3]*a[3][1] - a[3][3]*a[2][1]; |
---|
159 | a[2][2] = a[3][3]*a[1][1] - a[1][3]*a[3][1]; |
---|
160 | a[3][2] = a[1][3]*a[2][1] - a[2][3]*a[1][1]; |
---|
161 | |
---|
162 | /* normalize j, k */ |
---|
163 | s = sqrt(DSQR(a[1][3]) + DSQR(a[2][3]) + DSQR(a[3][3])); |
---|
164 | for (i=1; i<=3; i++) a[i][3] /= s; |
---|
165 | s = sqrt(DSQR(a[1][2]) + DSQR(a[2][2]) + DSQR(a[3][2])); |
---|
166 | for (i=1; i<=3; i++) a[i][2] /= s; |
---|
167 | |
---|
168 | /* cross j, k to produce i */ |
---|
169 | a[1][1] = a[2][2]*a[3][3] - a[3][2]*a[2][3]; |
---|
170 | a[2][1] = a[3][2]*a[1][3] - a[1][2]*a[3][3]; |
---|
171 | a[3][1] = a[1][2]*a[2][3] - a[2][2]*a[1][3]; |
---|
172 | |
---|
173 | } |
---|
174 | |
---|
175 | void dframeinverse( double **E, double **Ein ) /* form inverse frame */ |
---|
176 | { |
---|
177 | double a[6], b[6]; |
---|
178 | int i, j; |
---|
179 | |
---|
180 | validate_dvector( a, 1, 4); |
---|
181 | validate_dvector( b, 1, 4); |
---|
182 | |
---|
183 | for (i=1; i<=3; i++) { |
---|
184 | for (j=1; j<=3; j++) { |
---|
185 | Ein[i][j] = E[j][i]; |
---|
186 | } |
---|
187 | Ein[4][i] = 0.0; |
---|
188 | Ein[i][4] = 0.0; |
---|
189 | } |
---|
190 | Ein[4][4] = 1.0; |
---|
191 | |
---|
192 | dgetcolumn(E, 4, a, 4); |
---|
193 | dmvmult(Ein, 4, 4, a, 4, b); |
---|
194 | for (i=1; i<=3; i++) Ein[i][4] = -b[i]; |
---|
195 | } |
---|
196 | |
---|
197 | |
---|
198 | void dcross4 ( double *a, double *b, double *c ) /* cross two 4-vectors */ |
---|
199 | { |
---|
200 | double x, y, z; |
---|
201 | |
---|
202 | #ifdef V_CHECK |
---|
203 | if ( !valid_d4vector_b( a ) ) |
---|
204 | nrerror("Invalid vector 1: dcross4\n"); |
---|
205 | if ( !valid_d4vector_b( b ) ) |
---|
206 | nrerror("Invalid vector 2: dcross4\n"); |
---|
207 | if ( !valid_d4vector_b( c ) ) |
---|
208 | nrerror("Invalid vector 3: dcross4\n"); |
---|
209 | #endif |
---|
210 | x = a[2]*b[3] - a[3]*b[2]; |
---|
211 | y = a[3]*b[1] - a[1]*b[3]; |
---|
212 | z = a[1]*b[2] - a[2]*b[1]; |
---|
213 | c[1] = x; |
---|
214 | c[2] = y; |
---|
215 | c[3] = z; |
---|
216 | c[4] = 0.0; |
---|
217 | } |
---|
218 | |
---|
219 | double column_dot( double **G1, int col1, double **G2, int col2 ) |
---|
220 | /* form vector dot product of two columns (1st 3 elements only) */ |
---|
221 | { |
---|
222 | int i; |
---|
223 | double sum; |
---|
224 | |
---|
225 | #ifdef V_CHECK |
---|
226 | if ( !valid_dframe( G1 ) ) |
---|
227 | nrerror("Invalid 1st frame: column_dot\n"); |
---|
228 | if ( !valid_dframe( G2 ) ) |
---|
229 | nrerror("Invalid 2nd frame: column_dot\n"); |
---|
230 | #endif |
---|
231 | sum = 0.0; |
---|
232 | for ( i=1; i<=3; i++ ) sum += G1[i][col1]*G2[i][col2]; |
---|
233 | return(sum); |
---|
234 | } |
---|
235 | |
---|
236 | void make_d_omega( double **G1, double **G2, double *w) |
---|
237 | /* find small rotational difference between two frames G1, G2 */ |
---|
238 | /* ref: Shear Magic 4A.27 p129 */ |
---|
239 | { |
---|
240 | #ifdef V_CHECK |
---|
241 | if ( !valid_dframe( G1 ) ) |
---|
242 | nrerror("Invalid 1st frame: make_d_omega\n"); |
---|
243 | if ( !valid_dframe( G2 ) ) |
---|
244 | nrerror("Invalid 2nd frame: make_d_omega\n"); |
---|
245 | #endif |
---|
246 | w[1] = column_dot( G1, 3, G2, 2 ); |
---|
247 | w[2] = column_dot( G1, 1, G2, 3 ); |
---|
248 | w[3] = column_dot( G1, 2, G2, 1 ); |
---|
249 | w[4] = 0.0; |
---|
250 | } |
---|
251 | |
---|
252 | void compare_link_frames( double **G1, double **G2, double *wp, double *wr, double *ww ) |
---|
253 | { |
---|
254 | double wrot[6], z1[6], z2[6], zz[6]; |
---|
255 | |
---|
256 | validate_dvector(wrot, 1, 4); |
---|
257 | validate_dvector(z1, 1, 4); |
---|
258 | validate_dvector(z2, 1, 4); |
---|
259 | validate_dvector(zz, 1, 4); |
---|
260 | |
---|
261 | #ifdef V_CHECK |
---|
262 | if ( !valid_dframe( G1 ) ) |
---|
263 | nrerror("Invalid 1st frame: compare_link_frames\n"); |
---|
264 | if ( !valid_dframe( G2 ) ) |
---|
265 | nrerror("Invalid 2nd frame: compare_link_frames\n"); |
---|
266 | #endif |
---|
267 | *wp = *wr = *ww = 0.0; |
---|
268 | dgetcolumn( G1, Z, z1, 4 ); /* z axis comparison by cross product */ |
---|
269 | dgetcolumn( G2, Z, z2, 4 ); |
---|
270 | dcross4( z1, z2, zz ); |
---|
271 | *wr = dvmag( zz, 3 ); |
---|
272 | |
---|
273 | dgetcolumn( G1, U, z1, 4 ); /* origin comparison */ |
---|
274 | dgetcolumn( G2, U, z2, 4 ); |
---|
275 | dvsub( z1, 4, z2, zz ); |
---|
276 | *wp = dvmag( zz, 3 ); |
---|
277 | |
---|
278 | /* printf("wp, wr, ww: %8.4lf %8.4lf %8.4lf\n", *wp, *wr, *ww ); */ |
---|
279 | } |
---|
280 | |
---|
281 | void compare_frames( double **G1, double **G2, double *wp, double *wr, double *ww ) |
---|
282 | { |
---|
283 | double wrot[6], z1[6], z2[6], zz[6]; |
---|
284 | |
---|
285 | validate_dvector(wrot, 1, 4); |
---|
286 | validate_dvector(z1, 1, 4); |
---|
287 | validate_dvector(z2, 1, 4); |
---|
288 | validate_dvector(zz, 1, 4); |
---|
289 | |
---|
290 | #ifdef V_CHECK |
---|
291 | if ( !valid_dframe( G1 ) ) |
---|
292 | nrerror("Invalid 1st frame: compare_frames\n"); |
---|
293 | if ( !valid_dframe( G2 ) ) |
---|
294 | nrerror("Invalid 2nd frame: compare_frames\n"); |
---|
295 | #endif |
---|
296 | *wp = *wr = *ww = 0.0; |
---|
297 | make_d_omega( G1, G2, wrot ); |
---|
298 | *wr = dvmag( wrot, 3 ); |
---|
299 | *wp = sqrt( DSQR(G1[X][U] - G2[X][U]) |
---|
300 | + DSQR(G1[Y][U] - G2[Y][U]) |
---|
301 | + DSQR(G1[Z][U] - G2[Z][U]) ); |
---|
302 | *ww = sqrt( DSQR(*wr) + DSQR(*wp) ); |
---|
303 | /* printf("wp, wr, ww: %8.4lf %8.4lf %8.4lf\n", *wp, *wr, *ww ); */ |
---|
304 | } |
---|
305 | |
---|
306 | /*------------------------------------------------------------*/ |
---|
307 | void FindAxis(double **T1, double **T2, double *axis, |
---|
308 | double *sphi, double *cphi,int *twitching) |
---|
309 | { |
---|
310 | /* Routine to find the axis of rotation between two frames and the angle between them. |
---|
311 | */ |
---|
312 | |
---|
313 | static double **FF; |
---|
314 | static int done_once = False; |
---|
315 | double ca[6],cb[6],cc[6],cd[6]; /* workspace vectors */ |
---|
316 | double da[6],db[6],dc[6],dd[6]; /* workspace vectors */ |
---|
317 | double d1, d2, d3, dmax; |
---|
318 | |
---|
319 | validate_dvector(ca, 1, 4); |
---|
320 | validate_dvector(cb, 1, 4); |
---|
321 | validate_dvector(cc, 1, 4); |
---|
322 | validate_dvector(cd, 1, 4); |
---|
323 | validate_dvector(da, 1, 4); |
---|
324 | validate_dvector(db, 1, 4); |
---|
325 | validate_dvector(dc, 1, 4); |
---|
326 | validate_dvector(dd, 1, 4); |
---|
327 | |
---|
328 | if (!done_once) { |
---|
329 | FF = dmatrix( 1, 4, 1, 4 ); |
---|
330 | done_once = True; |
---|
331 | } |
---|
332 | |
---|
333 | #ifdef V_CHECK |
---|
334 | if ( !valid_dframe( T1 ) ) |
---|
335 | nrerror("Invalid 1st frame: FindAxis\n"); |
---|
336 | if ( !valid_dframe( T2 ) ) |
---|
337 | nrerror("Invalid 2nd frame: FindAxis\n"); |
---|
338 | if ( !valid_d4vector_b( axis ) ) |
---|
339 | nrerror("Invalid axis vector: FindAxis\n"); |
---|
340 | #endif |
---|
341 | |
---|
342 | *twitching = False; |
---|
343 | |
---|
344 | /* Find axis of rotation */ |
---|
345 | |
---|
346 | dmsub(T2, 4, 4, T1, FF); /* Difference of transforms */ |
---|
347 | dgetcolumn(FF, X, ca, 4); |
---|
348 | dgetcolumn(FF, Y, cb, 4); |
---|
349 | dgetcolumn(FF, Z, cc, 4); |
---|
350 | dcross4( ca, cb, da ); |
---|
351 | dcross4( cb, cc, db ); |
---|
352 | dcross4( cc, ca, dc ); |
---|
353 | d1 = dvmnorm( da, 3 ); |
---|
354 | d2 = dvmnorm( db, 3 ); |
---|
355 | d3 = dvmnorm( dc, 3 ); |
---|
356 | if ( d1 < d2 ) { |
---|
357 | if ( d2 < d3 ) { |
---|
358 | dvcopy( dc, 4, axis ); |
---|
359 | dmax = d3; |
---|
360 | } |
---|
361 | else { |
---|
362 | dvcopy( db, 4, axis ); |
---|
363 | dmax = d2; |
---|
364 | } |
---|
365 | } |
---|
366 | else { |
---|
367 | if ( d1 < d3 ) { |
---|
368 | dvcopy( dc, 4, axis ); |
---|
369 | dmax = d3; |
---|
370 | } |
---|
371 | else { |
---|
372 | dvcopy( da, 4, axis ); |
---|
373 | dmax = d1; |
---|
374 | } |
---|
375 | } |
---|
376 | /* printf("dmax = %12.6lf\n", dmax); */ |
---|
377 | if( dmax < 0.000001 ) { /* there is no rotation, or we have a problem */ |
---|
378 | d1 = column_dot( T1, X, T2, X); |
---|
379 | d2 = column_dot( T1, Y, T2, Y); |
---|
380 | d3 = column_dot( T1, Z, T2, Z); |
---|
381 | if( d1<0.5 || d2<0.5 || d3<0.5 ) { /* problem */ |
---|
382 | fprintf( stderr, "FindAxis: Frames opposed to each other\n"); |
---|
383 | dgetcolumn(T1, Z, axis, 4); /* Set axis to k vector and angle */ |
---|
384 | *cphi = 1.0; /* to zero. */ |
---|
385 | *sphi = 0.0; |
---|
386 | return; |
---|
387 | } |
---|
388 | else { /* Orientation change is small */ |
---|
389 | *twitching = True; |
---|
390 | ca[1] = column_dot( T1, 3, T2, 2 ); |
---|
391 | ca[2] = column_dot( T1, 1, T2, 3 ); |
---|
392 | ca[3] = column_dot( T1, 2, T2, 1 ); |
---|
393 | ca[4] = 0.0; |
---|
394 | d1 = dvmag( ca, 3 ); |
---|
395 | if ( d1 > 0.00000000000001 )d1 = dvmnorm( ca, 3 ); |
---|
396 | printf("Twitch\n"); |
---|
397 | dvcopy( ca, 4, axis ); |
---|
398 | *sphi = d1; |
---|
399 | *cphi = sqrt(1.0 - d1*d1); |
---|
400 | return; |
---|
401 | } |
---|
402 | } |
---|
403 | |
---|
404 | /* Find angle */ |
---|
405 | |
---|
406 | dgetcolumn(T1, X, ca, 4); |
---|
407 | dgetcolumn(T1, Y, cb, 4); |
---|
408 | dgetcolumn(T2, X, da, 4); |
---|
409 | dgetcolumn(T2, Y, db, 4); |
---|
410 | if( dvdot(axis, 3, cb) > 0.9) { /* rotation axis is close to j - use */ |
---|
411 | dcross4( axis, ca, cc ); /* change in i axis to find angle */ |
---|
412 | dcross4( axis, da, cd ); |
---|
413 | } |
---|
414 | else { /* use */ |
---|
415 | dcross4( axis, cb, cc ); /* change in j axis to find angle */ |
---|
416 | dcross4( axis, db, cd ); |
---|
417 | } |
---|
418 | d1 = dvmag( cc, 3 ); |
---|
419 | d2 = dvmag( cd, 3 ); |
---|
420 | /* fprintf( stdout, "d1, d2: %11.6lf %11.6lf \n", d1, d2 ); */ |
---|
421 | if ( d1 == 0.0 && d2 == 0.0 ) { |
---|
422 | *cphi = 1.0; |
---|
423 | *sphi = 0.0; |
---|
424 | } |
---|
425 | else { |
---|
426 | dvsmy( cc, 3, 1.0/d1, cc ); |
---|
427 | dvsmy( cd, 3, 1.0/d2, cd ); /* normalize */ |
---|
428 | *cphi = dvdot(cc, 3, cd); /* cos(phi) given by dot of two vectors */ |
---|
429 | dcross4( cc, cd, dd ); |
---|
430 | *sphi = dvmag(dd, 3); |
---|
431 | if( dvdot(dd, 3, axis) < 0.0) { |
---|
432 | *sphi = -(*sphi); |
---|
433 | } |
---|
434 | } |
---|
435 | return; |
---|
436 | } |
---|
437 | |
---|
438 | /* ---------------------------------------------------------------------------- */ |
---|
439 | /* Functions to rotate vector or frame about an arbitrary axis */ |
---|
440 | |
---|
441 | void RotateVector(double *v, double *axis, double sphi, double cphi, double *y) |
---|
442 | { |
---|
443 | /* v= sin(angle)(axis X v) + Cos(angle)v + (1-Cos(angle))(v . axis) * axis |
---|
444 | See Robots for shearing sheep pg 128 |
---|
445 | Note that v and y can be the same |
---|
446 | */ |
---|
447 | |
---|
448 | double db[6],dd[6]; |
---|
449 | double scal; |
---|
450 | |
---|
451 | validate_dvector( db, 1, 4); |
---|
452 | validate_dvector( dd, 1, 4); |
---|
453 | scal = (1.0 - cphi) * dvdot(axis, 3, v); |
---|
454 | dvsmy( axis, 3, scal, dd ); |
---|
455 | dcross4( axis, v, db); |
---|
456 | dvpiv( db, 3, sphi, dd, dd ); |
---|
457 | dvpiv( v, 3, cphi, dd, y ); |
---|
458 | y[4] = 0.0; |
---|
459 | } |
---|
460 | |
---|
461 | void RotateFrame(double **T1, double *axis, double sphi, double cphi, |
---|
462 | double **T2) |
---|
463 | { |
---|
464 | /* Routine to rotate coord frame T1 about axis by angle phi to give T2 |
---|
465 | using quaternion rotation. Note axis, T1 and T2 are all defined in |
---|
466 | terms of one (world) coord frame - axis is NOT defined in terms of T1. |
---|
467 | It is assumed that T1, and hence T2, are orthogonalized. T1 can be |
---|
468 | the same array as T2. |
---|
469 | sphi, cphi are sin and cosine of 'phi'. |
---|
470 | Peter Kovesi May 1985, adapted to C James Trevelyan July 1994 |
---|
471 | */ |
---|
472 | /* Temporary local data */ |
---|
473 | double ca[6],cb[6],cc[6],cd[6]; /* workspace vectors */ |
---|
474 | validate_dvector( ca, 1, 4); |
---|
475 | validate_dvector( cb, 1, 4); |
---|
476 | validate_dvector( cc, 1, 4); |
---|
477 | validate_dvector( cd, 1, 4); |
---|
478 | |
---|
479 | #ifdef V_CHECK |
---|
480 | if ( !valid_dframe( T1 ) ) |
---|
481 | nrerror("Invalid 1st frame: RotateFrame\n"); |
---|
482 | if ( !valid_dframe( T2 ) ) |
---|
483 | nrerror("Invalid 2nd frame: RotateFrame\n"); |
---|
484 | if ( !valid_d4vector_b( axis ) ) |
---|
485 | nrerror("Invalid axis vector: RotateFrame\n"); |
---|
486 | #endif |
---|
487 | |
---|
488 | dgetcolumn( T1, X, ca, 4 ); |
---|
489 | RotateVector(ca, axis, sphi, cphi, cb); |
---|
490 | dgetcolumn( T1, Y, ca, 4 ); |
---|
491 | RotateVector(ca, axis, sphi, cphi, cc); |
---|
492 | dcross4( cb, cc, cd); /* T2's k axis */ |
---|
493 | dputcolumn( cb, 4, T2, X ); |
---|
494 | dputcolumn( cc, 4, T2, Y ); |
---|
495 | dputcolumn( cd, 4, T2, Z ); |
---|
496 | dgetcolumn( T1, U, ca, 4 ); |
---|
497 | dputcolumn( ca, 4, T2, U ); |
---|
498 | } |
---|
499 | |
---|