home *** CD-ROM | disk | FTP | other *** search
/ RISC DISC 3 / RISC_DISC_3.iso / resources / etexts / gems / gemsiii / sqfinal.c < prev    next >
C/C++ Source or Header  |  1992-12-14  |  10KB  |  318 lines
  1. /*
  2.  * This code calculates the volume, mass, and inertia tensors of 
  3.  * superquadric ellipsoids and toroids. The code includes methods
  4.  * to numerically compute gamma functions and beta functions
  5.  */
  6.  
  7. #include <stdio.h>
  8. #include <math.h>
  9.  
  10. /*
  11.  * The following function, abgam() is based on a continued fraction numerical
  12.  * method found in Abremowitz and Stegun, Handbook of Mathematical Functions
  13.  * 
  14.  */
  15. double  abgam (x)
  16. double  x;
  17. {
  18.   double  gam[10],
  19.           temp;
  20.  
  21.   gam[0] = 1./ 12.;
  22.   gam[1] = 1./ 30.;
  23.   gam[2] = 53./ 210.;
  24.   gam[3] = 195./ 371.;
  25.   gam[4] = 22999./ 22737.;
  26.   gam[5] = 29944523./ 19733142.;
  27.   gam[6] = 109535241009./ 48264275462.;
  28.   temp = 0.5*log (2*M_PI) - x + (x - 0.5)*log (x)
  29.     + gam[0]/(x + gam[1]/(x + gam[2]/(x + gam[3]/(x + gam[4] /
  30.       (x + gam[5]/(x + gam[6]/x))))));
  31.  
  32.   return temp;
  33. }
  34.  
  35.  
  36.  
  37. /* 
  38.  * A method to compute the gamma() function.
  39.  *
  40.  */  
  41. double  gamma (x)
  42. double  x;
  43. {
  44.   double  result,
  45.           abgam ();
  46.   result = exp (abgam (x + 5))/(x*(x + 1)*(x + 2)*(x + 3)*(x + 4));
  47.   return result;
  48. }
  49.  
  50. /* 
  51.  * A method to compute the beta() function.
  52.  */  
  53. double  beta (m, n)
  54. double  m,
  55.         n;
  56. {
  57.   double  gamma ();
  58.   return (gamma (m)*gamma (n)/gamma (m + n));
  59. }
  60.  
  61.  
  62. /* 
  63.  * A method to compute the volume of a superquadric ellipsoid
  64.  * with axis lengths a1, a2, a3, north-south exponent n
  65.  * and east-west exponent e
  66.  */  
  67. double  sqellipvol (a1, a2, a3, n, e)
  68. double  a1,                               /* x radius */ 
  69.         a2,                               /* y radius */
  70.         a3,                               /* z radius */
  71.         n,                                /* north-south param */
  72.         e;                                /* east-west param */
  73. {
  74.   double  beta ();
  75.   return ((2./ 3.)*a1*a2*a3*e*n*beta (e/2., e/2.)*beta (n, n/2.));
  76. }
  77.  
  78.  
  79. /* 
  80.  * A method to compute the volume of a superquadric toroid
  81.  * with axis lengths a1, a2, a3, north-south exponent n
  82.  * east-west exponent e, and hole parameter alpha
  83.  */  
  84. double  sqtoroidvol (a1, a2, a3, n, e, alpha)
  85. double  a1,                               /* x radius */ 
  86.         a2,                               /* y radius */
  87.         a3,                               /* z radius */
  88.         n,                                /* north-south param */
  89.         e,                                /* east-west param */
  90.         alpha;                            /* torus hole-size parameter */
  91. {
  92.   return (2.*a1*a2*a3*alpha*e*n*beta (e/2., e/2.)*beta (n/2., n/2.));
  93.  
  94. }
  95.  
  96.  
  97.  
  98. /* 
  99.  * A procedure to print the inertia tensor of a canonical superquadric 
  100.  * ellipsoid in its body coordinate system. 
  101.  */  
  102. void sq_ellipsoid_tensor (a1, a2, a3, e, n)
  103. double  a1,
  104.         a2,
  105.         a3,
  106.         e,
  107.         n;
  108. {
  109.   double  iellip[3][3],
  110.           i1E,
  111.           i2E,
  112.           i3E;
  113.   i1E = (2./ 5.)* a1*a1*a1*a2*a3*e*n*beta(3.* e/2., e/2.)*beta(2.* n, n/2.);
  114.   i2E = (2./ 5.)* a1*a2*a2*a2*a3*e*n*beta(e/2., 3.* e/2.)*beta(2.* n, n/2.);
  115.   i3E = (2./ 5.)* a1*a2*a3*a3*a3*e*n*beta(e/2., e/2.)*beta(n, 3.* n/2.);
  116.  
  117.   iellip[0][0] = 0;
  118.   iellip[1][0] = 0;
  119.   iellip[2][0] = 0;
  120.   iellip[0][1] = 0;
  121.   iellip[1][1] = 0;
  122.   iellip[2][1] = 0;
  123.   iellip[0][2] = 0;
  124.   iellip[1][2] = 0;
  125.   iellip[2][2] = 0;
  126.   
  127.   iellip[0][0] = i2E + i3E;
  128.   iellip[1][1] = i1E + i3E;
  129.   iellip[2][2] = i1E + i2E;
  130.   
  131.   printf ("ellipsoid inertia tensor in body coordinates\n");
  132.  
  133.   printf ("  iellip1 = %f %f %f \n", iellip[0][0], iellip[1][0], iellip[2][0]);
  134.   printf ("  iellip2 = %f %f %f \n", iellip[0][1], iellip[1][1], iellip[2][1]);
  135.   printf ("  iellip3 = %f %f %f \n", iellip[0][2], iellip[1][2], iellip[2][2]);
  136.  
  137. }
  138.  
  139.  
  140. /* 
  141.  * A procedure to print the inertia tensor of a canonical superquadric 
  142.  * toroid in its body coordinate system. 
  143.  */  
  144. void sq_toroid_tensor ( a1, a2, a3, e, n, alpha)
  145. double  a1,
  146.         a2,
  147.         a3,
  148.         e,
  149.         n,
  150.         alpha;
  151. {
  152.   double  itor[3][3],
  153.           i1T,
  154.           i2T,
  155.           i3T;
  156.   i1T = a1*a1*a1*a2*a3*alpha*e*n*beta(3*e/2,e/2)*(2*alpha*alpha*beta(n/2,n/2)+
  157.       3*beta(3*n/2,n/2));
  158.   i2T = a1*a2*a2*a2*a3*alpha*e*n*beta(e/2,3*e/2)*(2*alpha*alpha*beta(n/2,n/2)+
  159.       3*beta(3*n/2,n/2));
  160.   i3T = a1*a2*a3*a3*a3*alpha*e*n*beta(e/2,e/2)*beta(n/2,3*n/2);
  161.  
  162.     itor[0][0] = 0;
  163.     itor[1][0] = 0;
  164.     itor[2][0] = 0;
  165.     itor[0][1] = 0;
  166.     itor[1][1] = 0;
  167.     itor[2][1] = 0;
  168.     itor[0][2] = 0;
  169.     itor[1][2] = 0;
  170.     itor[2][2] = 0;
  171.     itor[0][0] = i2T + i3T;
  172.     itor[1][1] = i1T + i3T;
  173.     itor[2][2] = i1T + i2T;
  174.     printf ("toroid inertia tensor in body coordinates\n");
  175.     printf ("  itor1  = %f %f %f \n", itor[0][0], itor[1][0], itor[2][0]);
  176.     printf ("  itor2  = %f %f %f \n", itor[0][1], itor[1][1], itor[2][1]);
  177.     printf ("  itor3  = %f %f %f \n", itor[0][2], itor[1][2], itor[2][2]);
  178. }
  179.  
  180. /* 
  181.  * A procedure to print the inertia tensor components in world coordinates,
  182.  * given an inertia tensor in body coordinates, and the 3x3 rotation matrix
  183.  * which rotates body vectors into world coordinates.  
  184.  */  
  185. void iworld (Ibody, R)
  186. double  Ibody[3][3],
  187.          R[3][3];
  188. {
  189.   double  Iworld[3][3];
  190.  Iworld[0][0] =
  191.     (R[0][0]*Ibody[0][0]+R[0][1]*Ibody[1][0]+R[0][2]*Ibody[2][0])*R[0][0] +
  192.     (R[0][0]*Ibody[0][1]+R[0][1]*Ibody[1][1]+R[0][2]*Ibody[2][1])*R[0][1] +
  193.     (R[0][0]*Ibody[0][2]+R[0][1]*Ibody[1][2]+R[0][2]*Ibody[2][2])*R[0][2];
  194.   
  195.   Iworld[1][0] =
  196.      (R[1][0]*Ibody[0][0]+R[1][1]*Ibody[1][0]+R[1][2]*Ibody[2][0])*R[0][0]+
  197.      (R[1][0]*Ibody[0][1]+R[1][1]*Ibody[1][1]+R[1][2]*Ibody[2][1])*R[0][1]+
  198.      (R[1][0]*Ibody[0][2]+R[1][1]*Ibody[1][2]+R[1][2]*Ibody[2][2])*R[0][2];
  199.   
  200.   Iworld[2][0] =
  201.      (R[2][0]*Ibody[0][0]+R[2][1]*Ibody[1][0]+R[2][2]*Ibody[2][0])*R[0][0]+
  202.      (R[2][0]*Ibody[0][1]+R[2][1]*Ibody[1][1]+R[2][2]*Ibody[2][1])*R[0][1]+
  203.      (R[2][0]*Ibody[0][2]+R[2][1]*Ibody[1][2]+R[2][2]*Ibody[2][2])*R[0][2];
  204.  
  205.   Iworld[0][1] =
  206.      (R[0][0]*Ibody[0][0]+R[0][1]*Ibody[1][0]+R[0][2]*Ibody[2][0])*R[1][0]+
  207.      (R[0][0]*Ibody[0][1]+R[0][1]*Ibody[1][1]+R[0][2]*Ibody[2][1])*R[1][1]+
  208.      (R[0][0]*Ibody[0][2]+R[0][1]*Ibody[1][2]+R[0][2]*Ibody[2][2])*R[1][2];
  209.   
  210.   Iworld[1][1] =
  211.      (R[1][0]*Ibody[0][0]+R[1][1]*Ibody[1][0]+R[1][2]*Ibody[2][0])*R[1][0]+
  212.      (R[1][0]*Ibody[0][1]+R[1][1]*Ibody[1][1]+R[1][2]*Ibody[2][1])*R[1][1]+
  213.      (R[1][0]*Ibody[0][2]+R[1][1]*Ibody[1][2]+R[1][2]*Ibody[2][2])*R[1][2];
  214.   
  215.   Iworld[2][1] =
  216.      (R[2][0]*Ibody[0][0]+R[2][1]*Ibody[1][0]+R[2][2]*Ibody[2][0])*R[1][0]+
  217.      (R[2][0]*Ibody[0][1]+R[2][1]*Ibody[1][1]+R[2][2]*Ibody[2][1])*R[1][1]+
  218.      (R[2][0]*Ibody[0][2]+R[2][1]*Ibody[1][2]+R[2][2]*Ibody[2][2])*R[1][2];
  219.    
  220.   Iworld[0][2] =
  221.      (R[0][0]*Ibody[0][0]+R[0][1]*Ibody[1][0]+R[0][2]*Ibody[2][0])*R[2][0]+
  222.      (R[0][0]*Ibody[0][1]+R[0][1]*Ibody[1][1]+R[0][2]*Ibody[2][1])*R[2][1]+
  223.      (R[0][0]*Ibody[0][2]+R[0][1]*Ibody[1][2]+R[0][2]*Ibody[2][2])*R[2][2];
  224.   
  225.   Iworld[1][2] =
  226.      (R[1][0]*Ibody[0][0]+R[1][1]*Ibody[1][0]+R[1][2]*Ibody[2][0])*R[2][0]+
  227.      (R[1][0]*Ibody[0][1]+R[1][1]*Ibody[1][1]+R[1][2]*Ibody[2][1])*R[2][1]+
  228.      (R[1][0]*Ibody[0][2]+R[1][1]*Ibody[1][2]+R[1][2]*Ibody[2][2])*R[2][2];
  229.   
  230.   Iworld[2][2] =
  231.      (R[2][0]*Ibody[0][0]+R[2][1]*Ibody[1][0]+R[2][2]*Ibody[2][0])*R[2][0]+
  232.      (R[2][0]*Ibody[0][1]+R[2][1]*Ibody[1][1]+R[2][2]*Ibody[2][1])*R[2][1]+
  233.      (R[2][0]*Ibody[0][2]+R[2][1]*Ibody[1][2]+R[2][2]*Ibody[2][2])*R[2][2];
  234.   
  235.   printf ("toroid inertia tensor in body coordinates\n");
  236.   printf (" Iworld1 = %f %f %f \n", Iworld[0][0], Iworld[1][0], Iworld[2][0]);
  237.   printf (" Iworld2 = %f %f %f \n", Iworld[0][1], Iworld[1][1], Iworld[2][1]);
  238.   printf (" Iworld3 = %f %f %f \n", Iworld[0][2], Iworld[1][2], Iworld[2][2]);
  239. }
  240.  
  241. /*
  242.  * sgn(x) returns -1.0 or 1.0 for speed. 
  243.  * sgn(x) can return 0.0 on zero if you wish, depending on
  244.  * your convention 
  245.  */ 
  246. double sgn(x)
  247. double x;
  248. {
  249.   if (x <= 0.0) return (-1.0);
  250.   else return(1.0);
  251. } 
  252.   
  253.  
  254. /*
  255.  * computes position on the surface of a superquadric ellipsoid
  256.  * v goes from -Pi/2 to Pi/2; u  goes from -Pi to Pi. 
  257.  *
  258.  */ 
  259. void sqellipsoidposn(a1,a2,a3,n,e,alpha,u,v)
  260. double a1,a2,a3,n,e,alpha,u,v;
  261. {
  262.   double cu, su, cv, sv, x,y,z;
  263.   cu = cos(u);
  264.   su = sin(u);
  265.   cv = cos(v);
  266.   sv = cos(v);
  267.  
  268.   x = a1*(alpha + pow(cv,n))*pow(cu,e)*sgn(cu)*sgn(cv);
  269.   y = a2*(alpha + pow(cv,n))*pow(su,e)*sgn(su)*sgn(cv);
  270.   z = a3*pow(sv,n)*sgn(sv);
  271. }  
  272.  
  273.  
  274. /*
  275.  * computes position on the surface of a superquadric toroid
  276.  * u and v go from -Pi to Pi
  277.  */
  278.  
  279. void sqtoroidposn(a1,a2,a3,n,e,u,v)
  280. double a1,a2,a3,n,e,u,v;
  281. {
  282.   double cu, su, cv, sv, x,y,z;
  283.   cu = cos(u);
  284.   su = sin(u);
  285.   cv = cos(v);
  286.   sv = cos(v);
  287.  
  288.   x = a1*pow(cv,n)*pow(cu,e)*sgn(cu)*sgn(cv);
  289.   y = a2*pow(cv,n)*pow(su,e)*sgn(su)*sgn(cv);
  290.   z = a3*pow(sv,n)*sgn(sv);
  291.  
  292. }  
  293.  
  294. /*  
  295.  * A procedure to test some of the above code
  296.  * 
  297.  */ 
  298. main () {
  299.   printf (" gamma(1)= 1.0 = %12.10lf\n", gamma (1.0));
  300.   printf (" gamma(1/2)^2= Pi =%12.10lf\n", gamma (0.5)*gamma (0.5));
  301.   printf (" gamma(2)= 1.0 = %12.10lf\n", gamma (2.0));
  302.   printf (" gamma(3)= 2.0 = %12.10lf\n", gamma (3.0));
  303.   printf (" gamma(4)= 6.0 = %12.10lf\n", gamma (4.0));
  304.   printf("\n");
  305.   printf ("beta(1,1)= 1.0 = %12.10lf\n", beta (1.0, 1.0));
  306.   printf ("beta(1,1/2)= 2.0 = %12.10lf\n", beta (1.0, 0.5));
  307.   printf ("beta(1/2,1/2)= Pi = %12.10lf\n", beta (0.5, 0.5));
  308.   printf("\n");
  309.   printf ("sq ellipsoid volume/pi= 4/3 = %12.10lf\n",
  310.                       sqellipvol(1., 1., 1., 1., 1.)/M_PI);
  311.   printf ("sq toroid volume/pi^2 = 2.0 = %12.10lf\n",
  312.            sqtoroidvol (1., 1., 1., 1., 1., 1.)/M_PI/M_PI);
  313.   printf("\n");
  314.   sq_ellipsoid_tensor (1., 1., 1., 1., 1.);
  315.   sq_toroid_tensor (1., 1., 1., 1., 1., 1.);
  316. }
  317.  
  318.