source: TRACY3/trunk/tracy/tools/nrframe.c @ 3

Last change on this file since 3 was 3, checked in by zhangj, 12 years ago

Initiale import
  • Property svn:executable set to *
File size: 13.2 KB
Line 
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
27extern FILE *outfile;
28extern double **F1;
29#endif
30
31double **SCR;
32
33double **dframe( void )
34{
35   return ( dmatrix( 1, 4, 1, 4 ) );
36}
37
38double *d4vector( void )
39{
40   return ( dvector( 1, 4 ) );
41}
42
43void 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
63void 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
102void 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
121void 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
148void 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
175void 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
198void 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
219double 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
236void 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
252void 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
281void 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/*------------------------------------------------------------*/
307void 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
441void 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
461void 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
Note: See TracBrowser for help on using the repository browser.