/* Matrix and rotation subroutines */#include "kep.h"#include <stdio.h>#if __STDC__double zatan2(double, double);double fabs(double);double sqrt(double);double sin(double);double cos(double);double atan2(double, double);void midentity (double M[]);void mmpy3 (double A[], double B[], double C[]);void mrotate (int from, int to, double M[], double theta);void EtoM (double e[], double M[]);void epmat (double JD, double P[]);#elsedouble zatan2(), fabs(), sqrt(), sin(), cos(), atan2();void midentity();void mmpy3();void mrotate();void EtoM();void epmat();#endif#define N 3/* Construct the matrix that represents the composite of * successive rotations by the three Euler angles: *   first by an angle phi about the z axis, *   second by an angle theta about the new x axis, *   third by an angle psi about the final z axis. * The input array e[] contains the three angles in the order * phi, theta, psi.  The 3x3 output matrix M[][] is the  * composite transformation of the three rotations in * Cartesian coordinates. *   The matrix elements have been written out directly * in trigonometric form so that the derivation of the * inverse function MtoE() will be clear. */voidEtoM( e, M )double e[];double M[];{double a, b;double sinpsi, cospsi, sinth, costh, sinphi, cosphi;sinpsi = sin(e[2]);cospsi = cos(e[2]);sinth = sin(e[1]);costh = cos(e[1]);sinphi = sin(e[0]);cosphi = cos(e[0]);a = costh*sinphi;b = costh*cosphi;M[0] = cospsi*cosphi - a*sinpsi;  /* M[0][0] */M[1] = cospsi*sinphi + b*sinpsi;  /* M[0][1] */M[2] = sinpsi*sinth;M[3] = -sinpsi*cosphi - a*cospsi;  /* M[1][0] */M[4] = -sinpsi*sinphi + b*cospsi;M[5] = cospsi*sinth;M[6] = sinth*sinphi;  /* M[2][0] */M[7] = -sinth*cosphi;M[8] = costh;  /* M[2][2] */}/* Deduce the three Euler angles e[] * from the input coordinate rotation matrix M[][]. * This is done by trigonometry from the elements of M. * * Since *   M[2][1] = -sin(theta) * cos(phi) *   M[2][0] =  sin(theta) * sin(phi), * then phi = arctan( M[2][0]/M[2][1]. * * Similarly, *  M[0][2] = sin(psi) * sin(theta) *  M[1][2] = cos(psi) * sin(theta), * so psi = arctan( M[0][2]/M[1][2] ). * * Finally, *   M[2][2] = cos(theta) *   M[1][2]/cos(psi) = sin(theta), * which gives the arctangent of theta. */voidMtoE( M, e )double M[], e[];{double a, b, phi, theta, psi;/* phi = zatan2( -M[2][1], M[2][0] ); */phi = atan2(  M[2*3+0], -M[2*3+1]);a = M[0*3+2];b = M[1*3+2];/* psi = zatan2( b, a ); */psi = atan2( a, b );if( fabs(a) > fabs(b) )	a = a/sin(psi);else	a = b/cos(psi);/* theta = zatan2( M[2][2], a ); */theta = atan2( a, M[2*3+2]);e[0] = phi;e[1] = theta;e[2] = psi;}/* Display elements of the NxN matrix M. */voidprtmat( M )double M[];{int r, c;for( r=0; r<N; r++ )	{	for( c=0; c<N; c++ )		printf( "%18.9f ", M[N*r+c] );	printf( "\n" );	}printf( "\n" );}voidprtvec( v )double v[];{int i;for( i=0; i<N; i++ )	printf( "%18.9f ", v[i] );printf( "\n\n" );}/* Fill the array M with an NxN identity matrix. */voidmidentity( M )double M[];{double *p;int i;p = &M[0];for( i=0; i<(N*N); i++ )	*p++ = 0.0;for( i=0; i<N; i++ )	M[i*(N+1)] = 1.0;}/* Matrix transpose. * B, the output, can occupy the same storage locations as * A, the input. */voidmtransp( A, B )double A[], B[];{double x, y;int r, c, rc, cr;for( r=0; r<N; r++ )	{	for( c=0; c<=r; c++ )		{		rc = N*r+c;		cr = N*c+r;		x = A[rc];		y = A[cr];		B[rc] = y;		B[cr] = x;		}	}}/* 3x3 matrix multiply * C = A * B * A is on the left. * C may occupy the same storage as either A or B. */voidmmpy3( A, B, C )double A[], B[], C[];{double ans[9];double s;int i, r, c;for( r=0; r<3; r++ )	{	for( c=0; c<3; c++ )		{		s = 0.0;		for ( i=0; i<3; i++ )			s += A[3*r+i] * B[3*i+c];		ans[3*r+c] = s;		}	}for( i=0; i<9; i++ )	{	C[i] = ans[i];	}}/* Modify matrix M to include a rotation of theta radians * in the from-to plane.  The sense of the rotation is * from the "from" axis (labeled 0, 1, or 2) to the "to" axis. */voidmrotate( from, to, M, theta )int from, to;double M[];double theta;{double c;double R[N*N];if( from == to )	{	printf( "mrotate: from and to must be different\n" );	return;	}midentity( R );c = cos(theta);R[N*from+from] = c;R[N*to+to] = c;c = sin(theta);R[N*from+to] = c;R[N*to+from] = -c;mmpy3( R, M, M );}/* Modify the input matrix M to include a rotation * of theta radians about the x axis. */voidmrotx( M, theta )double M[];double theta;{mrotate( 1, 2, M, theta );}/* Modify the input matrix M to include a rotation * of theta radians about the y axis. */voidmroty( M, theta )double M[];double theta;{mrotate( 2, 0, M, theta );}/* Modify the input matrix M to include a rotation * of theta radians about the z axis. */voidmrotz( M, theta )double M[];double theta;{mrotate( 0, 1, M, theta );}/* Transform the input vector v by rotating the coordinate system * theta radians about the x axis. */voidrotx( v, theta )double v[];double theta;{double y, sinth, costh;sinth = sin(theta);costh = cos(theta);y = costh*v[1] + sinth*v[2];v[2] = -sinth*v[1] + costh*v[2];v[1] = y;}/* Transform the input vector v by rotating the coordinate system * theta radians about the y axis. */voidroty( v, theta )double v[];double theta;{double x, sinth, costh;sinth = sin(theta);costh = cos(theta);x = costh*v[0] - sinth*v[2];v[2] = sinth*v[0] + costh*v[2];v[0] = x;}/* Transform the input vector v by rotating the coordinate system * theta radians about the z axis. */voidrotz( v, theta )double v[];double theta;{double x, sinth, costh;sinth = sin(theta);costh = cos(theta);x = costh*v[0] + sinth*v[1];v[1] = -sinth*v[0] + costh*v[1];v[0] = x;}/* Return the largest element-by-element difference between * two NxN arrays A and B. */double l1diff( A, B )double *A, *B;{double x, max;int i;max = 0;for( i=0; i<N*N; i++ )	{	x = fabs( *A++ - *B++ );	if( x > max )		max = x;	}return( max );}/* The following subprogram, epmat( JD, P ), * constructs the precession matrix in ecliptic coordinates * to go from the epoch J2000.0 to the date JD.  Output P is the * 3x3 rotation matrix.  To go in the opposite direction, * from JD to J2000.0, the required matrix is just the transpose of P. * Note that the obliquity of Earth's equator is not required here * since the coordinates are ecliptic, not equatorial. */#if (DE403 | DE404 | LIB403 | DE405 | DE406 | DE406CD)/* James G. Williams, "Contributions to the Earth's obliquity rate,   precession, and nutation,"  Astron. J. 108, 711-72s4 (1994).   The values used here are slightly different, for DE403.  */  /* 0.000000   5028.791959   1.105414   0.000076  -0.000024 */static double pAcof[] = { -8.66e-10, -4.759e-8, 2.424e-7, 1.3095e-5, 1.7451e-4, -1.8055e-3, -0.235316, 0.076, 110.5414, 50287.91959};/* Pi from Williams' 1994 paper, in radians.  No change in DE403.  *//* 629543.967373   -867.919986   0.153382   0.000026  -0.000004 */static double nodecof[] = {6.6402e-16, -2.69151e-15, -1.547021e-12, 7.521313e-12, 1.9e-10, -3.54e-9, -1.8103e-7,  1.26e-7,  7.436169e-5,-0.04207794833,  3.052115282424};/* pi from Williams' 1994 paper, in radians.  No change in DE403.  *//* 0.000000     46.997570  -0.033506  -0.000124   0.000000 */static double inclcof[] = {1.2147e-16, 7.3759e-17, -8.26287e-14, 2.503410e-13, 2.4650839e-11, -5.4000441e-11, 1.32115526e-9, -6.012e-7, -1.62442e-5, 0.00227850649, 0.0 };#else /* not DE403 *//* Accumulated precession in longitude.  Coefficients are from: * J. Laskar, "Secular terms of classical planetary theories * using the results of general theory," Astronomy and Astrophysics * 157, 59070 (1986). */static double pAcof[] = { -8.66e-10, -4.759e-8, 2.424e-7, 1.3095e-5, 1.7451e-4, -1.8055e-3, -0.235316, 0.07732, 111.1971, 50290.966 };/* Node and inclination of the earth's orbit computed from * Laskar's data as explained in the following paper: * P. Bretagnon and G. Francou, "Planetary theories in rectangular * and spherical variables. VSOP87 solutions," Astronomy and * Astrophysics 202, 309-315 (1988). */static double nodecof[] = {6.6402e-16, -2.69151e-15, -1.547021e-12, 7.521313e-12, 6.3190131e-10, -3.48388152e-9, -1.813065896e-7, 2.75036225e-8, 7.4394531426e-5,-0.042078604317, 3.052112654975 };static double inclcof[] = {1.2147e-16, 7.3759e-17, -8.26287e-14, 2.503410e-13, 2.4650839e-11, -5.4000441e-11, 1.32115526e-9, -5.998737027e-7, -1.6242797091e-5, 0.002278495537, 0.0 };#endif /* not DE403 *//* radians per arc second */#define STR 4.8481368110953599359e-6voidepmat( JD, P )double JD;double P[];{double T, pA, i, Omega;double e[3];double *p;int j;/* Thousands of years from J2000.0.  */T = (JD - 2451545.0)/365250.0;/* Accumulated precession in longitude.  */p = pAcof;pA = *p++;for( j=0; j<9; j++ )	pA = pA * T + *p++;pA *= STR * T;/* Node of the moving ecliptic on the J2000 ecliptic.  */p = nodecof;Omega = *p++;for( j=0; j<10; j++ )	Omega = Omega * T + *p++;/* Inclination of the moving ecliptic to the J2000 ecliptic.  */p = inclcof;i = *p++;for( j=0; j<10; j++ )	i = i * T + *p++;/* Set up the Euler angles. */e[0] = Omega;e[1] = i;e[2] = -(Omega + pA );/* Construct the rotation matrix  to go from J2000 to JD  */EtoM( e, P );}