// htm1.cpp : Defines the entry point for the console application. // // "HTM1" denotes Heliocentric Trajectory Model, Version 1 // // This two-body heliocentric inertial (HCI) ecliptic cartesian // solar system object path propagation program was spun off from // my ECI equatorial rocket trajectory modeling program MTM2 in // order to re-use the math and astrodynamic library functions. // // Also to re-use the overall logic of propagating via f and g // functions of Stumpff's c-functions. // // Roger L. Mansfield // 2019 March 1 // // #include "stdafx.h" (Visual C++ 2010 Express needs this.) /* HTM1 inputs the cartesian state vector of a solar system object. It propagates the path (trajectory) via universal variables, i.e., f and g functions of Stumpff's c-functions. It also does a transformation of the initial state vector to conic elements. Processing Summary: 1. Input HCI ecliptic cartesian state vector, start time, stop time, and time step. 2. Propagate celestial body's trajectory via f and g functions of Stumpff's c-functions, as implemented in function FG. Output time since epoch, HCI x, y, z, xdot, ydot, zdot, and number of iterations of Stumpff's form of the uniform Kepler equation (see Topics in Astrodynamics, Chapter 14, p. 266). 3. Transform position and velocity vectors to conic elements by calling function CARTESIAN_TO_CONIC. Output conic elements. ************************************************************ Summary of HTM1's functions and procedures. Function Description ABS Computes absolute value of x. EXP Computes exp(x). LN Computes ln(x). SQR Raises x to second power. CUB Raises x to third power. SIN Computes sin(x). COS Computes cos(x). TAN Computes tan(x). ASIN Computes arc-sine of x. ACOS Computer arc-cosine of x. ARCTAN Computes arc-tangent of x. ATAN2 Computes arc-tangent of (x,y). COSH Computes cosh(x). SINH Computes sinh(x). ATANH Computes hyperbolic arc-tangent. DOT Computes scalar product of two 3-vectors. CROSS Computes vector product of two 3-vectors. XMOD2PI Computes x modulo 2pi. C Computes first four c-functions of argument x. CFUNCS Computes first six c-functions by series and recursion. VFN Computes "fictitious time" S as function of true anomaly, V. FG Computes position and velocity as functions of f and g functions of Stumpff's c-functions. CARTESIAN_TO_CONIC Computes conic orbital elements as functions of position and velocity. main() Carries out steps 1, 2, and 3 in the Processing Summary outlined above. */ #include #include #include #include #include /* labels are a1000, a9000, a9999; */ #define EPSILON 1.0E-10 /* Approximation to zero. */ #define RADIAN 57.29577951 /* No. degrees in one radian. */ #define DEGREE 0.017453293 /* No. radians in one degree. */ #define HPI 1.570796327 /* One-half pi. */ #define PI 3.141592654 /* Pi. */ #define TPI 6.283185307 /* Twice pi. */ /* Define KE. */ #define KE 0.01720209895 /* Gaussian constant for Sun as primary particle, with units of AU^3/2 per day. */ /* Define type "REAL" as double. */ typedef double REAL; /* Declare legacy globals from EG.PAS. */ REAL ARG, ARGP, C0, C1,ENEG, ENEGO, F, FDOT, G, GDOT, KS, KSQ, RMAG, RMAGO, S, S2, S3, SIGMAO, SP, T, T1, TT, TAU, V; REAL Q, ECC, INCL, NODE, XA, XN, XM, PERIOD, TPE; REAL R[4], RO[4], RDOT[4], RDOTO[4]; REAL YEAR, DAYS, GHAO; char CH; /* Declare function prototypes. */ REAL ABS(REAL X); REAL EXP(REAL X); REAL LN(REAL X); REAL SQR(REAL X); REAL SQRT(REAL X); REAL CUB(REAL X); REAL SIN(REAL X); REAL COS(REAL X); REAL TAN(REAL X); REAL ASIN(REAL X); REAL ACOS(REAL X); REAL ARCTAN(REAL X); REAL ATAN2(REAL X, REAL Y); REAL COSH(REAL X); REAL SINH(REAL X); REAL ATANH(REAL X); REAL DOT(REAL A[4], REAL B[4]); void CROSS(REAL A[4], REAL B[4], REAL C[4]); REAL XMOD2PI(REAL X); REAL C(int N, REAL X); void CFUNCS(REAL Y, REAL *C0, REAL *C1, REAL *C2, REAL *C3, REAL *C4, REAL *C5); REAL VFN(); void FG(REAL RMAGO, REAL SIGMAO, REAL ENEGO, REAL T, REAL T1, REAL RO[4], REAL RDOTO[4], REAL *S, REAL *ARG, REAL *F, REAL *G, REAL *FDOT, REAL *GDOT, REAL *RMAG, REAL R[4], REAL RDOT[4], int *ITER); void CARTESIAN_TO_CONIC(); /* File descriptors and names */ FILE *INP, *OUT; /* Begin main program execution here. */ int main() { int I, ITER, N, NSTEPS; REAL TSTART, TSTOP, TSTEP; /* Open ./htm_inp.txt for input. */ if ((INP = fopen("./htm_inp.txt", "r")) == NULL) { printf("Can't open ./htm_inp.txt \n"); return -1; } /* Open ./htm_out.txt for output. */ if ((OUT = fopen("./htm_out.txt", "w")) == NULL) { printf("Can't open ./htm_out.txt \n"); return -2; } /* Output HTM program header. */ printf ( "\n"); printf ( "HELIOCENTRIC TRAJECTORY MODELING (HTM) PROGRAM, VERSION 1 \n"); printf ( "\n"); printf ( "TWO-BODY MODEL, UNIVERSAL VARIABLES\n"); fprintf(OUT, "HELIOCENTRIC TRAJECTORY MODELING (HTM) PROGRAM, VERSION 1 \n"); fprintf(OUT, "\n"); fprintf(OUT, "TWO-BODY MODEL, UNIVERSAL VARIABLES\n"); fprintf(OUT, "\n"); fprintf(OUT, " TIME SINCE "); fprintf(OUT, " X "); fprintf(OUT, " Y "); fprintf(OUT, " Z "); fprintf(OUT, " XDOT "); fprintf(OUT, " YDOT "); fprintf(OUT, " ZDOT NO.\n"); fprintf(OUT, " EPOCH, DAYS "); fprintf(OUT, " AU "); fprintf(OUT, " AU "); fprintf(OUT, " AU "); fprintf(OUT, " AU/DAY "); fprintf(OUT, " AU/DAY "); fprintf(OUT, " AU/DAY ITS\n "); fprintf(OUT, "\n"); /* Equate KS to KE and define KSQ. */ KS = KE; KSQ = KS*KS; /* *********************************************************** 1. Input HCI cartesian state vector, start time, stop time, and time step. *********************************************************** */ /* Input position components in AU. */ fscanf(INP, "%lf %lf %lf \n", &RO[1], &RO[2], &RO[3]); /* Input velocity components in AU/day. */ fscanf(INP, "%lf %lf %lf \n", &RDOTO[1], &RDOTO[2], &RDOTO[3]); /* Convert components to AU per ksday. */ for (I = 1; I <= 3; I++) RDOTO[I] = RDOTO[I]/KE; /* Input start, stop, and step times in days. */ fscanf(INP, "%lf %lf %lf \n", &TSTART, &TSTOP, &TSTEP); /* Initiate some program variables that do not change. */ RMAGO = SQRT(DOT(RO,RO)); SIGMAO = DOT(RO,RDOTO); ENEGO = 1.0/RMAGO - 0.5*DOT(RDOTO,RDOTO); /* *********************************************************** 2. Propagate celestial body's trajectory via f and g functions of Stumpff's c-functions, as implemented in function FG. Output time since epoch, HCI x, y, z, xdot, ydot, zdot, and number of iterations of Stumpff's form of the uniform Kepler equation (see Topics in Astrodynamics, Chapter 14, p. 266). *********************************************************** */ NSTEPS = (TSTOP - TSTART)/TSTEP; /* Initialize logic to use previous calculation of S to help with first estimate in current calculation of S -- see usage in procedure FG. */ RMAG = RMAGO; S = 0.0; /* T1 = 0.0; */ T1 = (TSTART - TSTEP)*KE; for (N = 0; N <= NSTEPS + 1; N++) { T = T1 + TSTEP*KE; FG(RMAGO,SIGMAO,ENEGO,T,T1,RO,RDOTO, &S,&ARG,&F,&G,&FDOT,&GDOT,&RMAG,R,RDOT,&ITER); TT = T/KE; RMAG = SQRT(DOT(R,R)); fprintf(OUT,"%14.5lf %14.8lf %14.8lf %14.8lf %14.8lf %14.8lf %14.8lf %6d \n", TT, R[1], R[2], R[3], RDOT[1]*KE, RDOT[2]*KE, RDOT[3]*KE, ITER); /* Save T as T1 for use in next step. S and RMAG will also be used to help make first estimate of S in next step -- see usage in procedure FG. */ T1 = T; } /* ************************************************************** 3. Transform position and velocity vectors to conic elements by calling function CARTESIAN_TO_CONIC. Output conic elements. ************************************************************** */ fprintf(OUT,"\n"); fprintf(OUT,"CONIC ELEMENTS AT EPOCH: \n"); fprintf(OUT,"\n"); for (I = 1; I <= 3; I++) { R[I] = RO[I]; RDOT[I] = RDOTO[I]; } CARTESIAN_TO_CONIC(); fprintf(OUT," PERIHELION RADIUS, AU %16.8lf \n", Q); fprintf(OUT," ECCENTRICITY %16.8lf \n", ECC); fprintf(OUT," TPE/K, DAYS %16.5lf \n", TPE); fprintf(OUT," INCLINATION, DEG %16.5lf \n", INCL*RADIAN); fprintf(OUT," NODE, DEG %16.5lf \n", NODE*RADIAN); fprintf(OUT," ARG. PERIHELION, DEG %16.5lf \n", ARGP*RADIAN); fprintf(OUT,"\n"); fclose(OUT); fclose(INP); printf("\n"); printf("PRESS TO EXIT ... \n"); CH = getch(); return 0; } REAL ABS(REAL X) /* ABS computes absolute value of X. */ { return fabs(X); } REAL EXP(REAL X) /* EXP computes the base-e exponential of X. */ { return exp(X); } REAL LN(REAL X) /* LN computes the natural logarithm of X. */ { return log(X); } REAL SQR(REAL X) /* SQR raises X to the second power. */ { return pow(X, 2.0); } REAL SQRT(REAL X) /* SQRT computes square root of x. */ { return sqrt(X); } REAL CUB(REAL X) /* CUB raises X to the third power. */ { return pow(X, 3.0); } REAL SIN(REAL X) /* SIN is the circular sine function. */ { return sin(X); } REAL COS(REAL X) /* COS is the circular cosine function. */ { return cos(X); } REAL TAN(REAL X) /* TAN is the circular tangent function. */ { return tan(X); } REAL ASIN(REAL X) /* ASIN is the circular Arc-sine function. */ { return asin(X); } REAL ACOS(REAL X) /* ACOS is the circular Arc-cos function. */ { return acos(X); } REAL ARCTAN(REAL X) /* ARCTAN is the circular Arc-tan function. */ { return atan(X); } REAL ATAN2(REAL X, REAL Y) /* ATAN2 is the four-quadrant Arc-tangent. */ { return atan2(Y,X); } REAL COSH(REAL X) /* COSH is the hyperbolic cosine function. */ { return cosh(X); } REAL SINH(REAL X) /* SINH is the hyperbolic sine function. */ { return sinh(X); } REAL ATANH(REAL X) /* ATANH is the hyperbolic arc-tangent. */ { return 0.5*log((1.0+X)/(1.0-X)); } REAL DOT(REAL A[4], REAL B[4]) /* DOT calculates scalar product of 3-vectors A and B. */ { return A[1]*B[1] + A[2]*B[2] + A[3]*B[3]; } void CROSS(REAL A[4], REAL B[4], REAL C[4]) /* CROSS calculates cross product of 3-vectors A and B. */ { C[1] = A[2]*B[3] - B[2]*A[3]; C[2] = B[1]*A[3] - A[1]*B[3]; C[3] = A[1]*B[2] - B[1]*A[2]; return; } REAL XMOD2PI(REAL X) /* XMOD2PI computes X modulo TPI. */ { REAL whole; return TPI*modf(X/TPI, &whole); } REAL C(int NC, REAL X) /* C provides the Stumpff functions c(n,x) for n = 0, 1, 2, 3, 4, or 5. Global variables: None. This procedure was modified January 8, 1992 to use series when ABS(X) <= 0.1. It was found that sin/cos and sinh/cosh representations agree with values computed by series and recursion to better than ten significant figures. Roger L. Mansfield, 31 December 1996. */ { /* Declare local variables. */ REAL c, Y; REAL C0, C1, C2, C3, C4, C5; c = 0.0; /* Compute c-functions by series and recursion and skip the rest. Debug ---> fprintf(OUT, "C-FUNCTION CALL, NC = %2d \n", NC); */ CFUNCS(X, &C0, &C1, &C2, &C3, &C4, &C5); if (NC == 0) return C0; if (NC == 1) return C1; if (NC == 2) return C2; if (NC == 3) return C3; if (NC == 4) return C4; if (NC == 5) return C5; /* Elliptical path: use circular function representation. */ if (X > 0.1) { Y = SQRT(X); switch(NC) { case (0) : { c = COS(Y); break; } case (1) : { c = SIN(Y)/Y; break; } case (2) : { c = (1.0 - COS(Y))/X; break; } case (3) : { c = (Y - SIN(Y))/(X*Y); break; } case (4) : { c = (COS(Y) - (1.0 - 0.5*X))/SQR(X); break; } case (5) : { c = (SIN(Y) - (Y - 0.16666666667*X*Y))/ (SQR(X)*Y); break; } default : { } } } /* Near-parabolic or parabolic path: use series representation. */ if (ABS(X) <= 0.1) { switch(NC) { case (0) : { c = (1.0-X*(1.0-X*(1.0-X*(1.0-X*(1.0-X*(1.0-X/ 132.0)/ 90.0)/ 56.0)/ 30.0)/ 12.0)/2.0); break; } case (1) : { c = (1.0-X*(1.0-X*(1.0-X*(1.0-X*(1.0-X*(1.0-X/ 156.0)/110.0)/ 72.0)/ 42.0)/ 20.0)/6.0); break; } case (2) : { c = (1.0-X*(1.0-X*(1.0-X*(1.0-X*(1.0-X*(1.0-X/ 182.0)/132.0)/ 90.0)/ 56.0)/ 30.0)/ 12.0)/ 2.0; break; } case (3) : { c = (1.0-X*(1.0-X*(1.0-X*(1.0-X*(1.0-X*(1.0-X/ 210.0)/156.0)/110.0)/ 72.0)/ 42.0)/ 20.0)/ 6.0; break; } case (4) : { c = (1.0-X*(1.0-X*(1.0-X*(1.0-X*(1.0-X*(1.0-X/ 240.0)/182.0)/132.0)/ 90.0)/ 56.0)/ 30.0)/ 24.0; break; } case (5) : { c = (1.0-X*(1.0-X*(1.0-X*(1.0-X*(1.0-X*(1.0-X/ 272.0)/210.0)/156.0)/110.0)/ 72.0)/ 42.0)/120.0; break; } default : { } } } /* Hyperbolic path: use hyperbolic function representation. */ if (-X > 0.1) { Y = SQRT(-X); switch (NC) { case (0) : { c = COSH(Y); break; } case (1) : { c = SINH(Y)/Y; break; } case (2) : { c = (1.0 - COSH(Y))/X; break; } case (3) : { c = (Y - SINH(Y))/(X*Y); break; } case (4) : { c = (COSH(Y) - (1.0 - 0.5*X))/SQR(X); break; } case (5) : { c = (SINH(Y) - (Y - 0.16666666667*X*Y))/ (SQR(X)*Y); break; } default : { } } } return c; } void CFUNCS(REAL Y, REAL *C0, REAL *C1, REAL *C2, REAL *C3, REAL *C4, REAL *C5) /* This procedure calculates the following c-functions Variable Stumpff's c-function C0 c(0,x) C1 c(1,x) C2 c(2,x) C3 c(3,x) C4 c(4,X) C5 c(5,X) as needed to calculate position, velocity, and state transition matrix for uniform differential correction of a cartesian state vector. Roger L. Mansfield, 31 December 1996. */ { /* Declare local variables. */ int N; REAL X, XM; XM = 0.1; /* Reduce X by quartering until it is less than parameter XM. */ X = Y; N = 0; while (ABS(X) >= XM) { X = X/4.0; N = N + 1; } /* Calculate c-functions of reduced X by series. */ *C5 = (1.0 - X*(1.0 - X*(1.0 - X*(1.0 - X*(1.0 - X* (1.0 - X/272.0)/210.0)/156.0)/110.0)/ 72.0)/ 42.0)/120.0; *C4 = (1.0 - X*(1.0 - X*(1.0 - X*(1.0 - X*(1.0 - X* (1.0 - X/240.0)/182.0)/132.0)/ 90.0)/ 56.0)/ 30.0)/ 24.0; *C3 = 0.16666666667 - X*(*C5); *C2 = 0.5 - X*(*C4); *C1 = 1.0 - X*(*C3); *C0 = 1.0 - X*(*C2); /* Calculate c-functions of original X by recursion. */ while (N != 0 ) { N = N - 1; *C5 = 0.0625*((*C2)*(*C3) + (*C4) + (*C5)); *C4 = 0.1250*((*C2)*(*C2) + (*C4) + (*C4)); *C3 = 0.2500*((*C1)*(*C2) + (*C3)); *C2 = 0.5000* (*C1)*(*C1); *C1 = (*C1)*(*C0); *C0 = 2.0000* (*C0)*(*C0) - 1.0; } return; } REAL VFN() /* VFN calculates fictitious time S as function of true anomaly V. Global variables: ENEG - Negative of total energy of two-particle system. Q - Radius of periapsis, typically in E.R. or AU ECC - Eccentricity. V - True anomaly. EPSILON - Convergence threshold. */ { REAL ARG, S; if (ABS(ENEG) < EPSILON) S = 2.0*SQRT(Q/(1.0+ECC))*TAN(0.5*V)/KS; else { if (ENEG > 0.0) { ARG = SQRT( 2.0*Q*ENEG/(1.0+ECC))*TAN(0.5*V); S = SQRT( 2.0/ENEG)*ARCTAN(ARG)/KS; } else { ARG = SQRT(-2.0*Q*ENEG/(1.0+ECC))*TAN(0.5*V); S = SQRT(-2.0/ENEG)* ATANH(ARG)/KS; } } return S; } void FG(REAL RMAGO, REAL SIGMAO, REAL ENEGO, REAL T, REAL T1, REAL RO[4], REAL RDOTO[4], REAL *S, REAL *ARG, REAL *F, REAL *G, REAL *FDOT, REAL *GDOT, REAL *RMAG, REAL R[4], REAL RDOT[4], int *ITER) /* FG computes F, G, FDOT, and GDOT from Goodyear's f-and-g functions, and from these, the position and velocity as functions of the position and velocity at epoch. Global variables: EPSILON - Convergence threshold. Note: This procedure accepts time in canonical units, and computes velocity in canonical units. Therefore, the Gaussian constant globals KS and KSQ are not used. Modified August 16, 1991 to calculate S using Laguerre- Conway iteration. */ { int I; REAL S2, S3, SP, XN, XN1, FN, SGN, FPP, DS; REAL C0, C1, C2, C3, C4, C5; XN = 5.0; XN1 = XN - 1.0; *S = *S + (T - T1)/(*RMAG); for (I = 1; I <= 20; I++) { S2 = SQR(*S); S3 = S2*(*S); *ARG = 2.0*ENEGO*S2; /* Compute c-functions by series and recursion. */ CFUNCS(*ARG, &C0, &C1, &C2, &C3, &C4, &C5); FN = RMAGO*(*S)*C1 + SIGMAO*S2*C2 + S3*C3 - T; *RMAG = RMAGO*C0 + SIGMAO*(*S)*C1 + S2*C2; if (*RMAG >= 0.0) SGN = 1.0; else SGN = -1.0; FPP = (1.0 - RMAGO*2.0*ENEGO)*(*S)*C1 + SIGMAO*C0; DS = -XN*FN/(*RMAG + SGN*SQRT(ABS(SQR(XN1*(*RMAG)) - XN*XN1*FN*FPP))); SP = (*S) + DS; if (ABS(DS) < EPSILON) goto a100; *S = SP; } a100: *ITER = I; *S = SP; S2 = SQR(SP); S3 = S2*SP; *ARG = 2.0*ENEGO*S2; CFUNCS(*ARG, &C0, &C1, &C2, &C3, &C4, &C5); *RMAG = RMAGO*C0 + SIGMAO*SP*C1 + S2*C2; *F = 1.0 - S2*C2/RMAGO; *G = T - S3*C3; *FDOT = -SP*C1/((*RMAG)*RMAGO); *GDOT = 1.0 - S2*C2/(*RMAG); for (I = 1; I <= 3; I++) { R[I] = (*F)*RO[I] + (*G)*RDOTO[I]; RDOT[I] = (*FDOT)*RO[I] + (*GDOT)*RDOTO[I]; } } void CARTESIAN_TO_CONIC() /* This procedure transforms position and velocity at some epoch to conic path elements at that epoch. Global variables: R - Position vector. RDOT - Velocity vector, in canonical units. Q - Periapsidal distance. ECC - Eccentricity. INCL - Inclination. NODE - Longitude/R.A. of ascending node. ARGP - Argument of periapsis. TPE - Time since periapsis passage (time of flight from periapsis to epoch). */ { int I; REAL ESINV, ECOSV, P, SQRTP, RDU, RDV, RMAG; REAL S, SINV, COSV, ARG, ARG1; REAL U[4], V1[4], W[4], H[4], P1[4], Q1[4]; RMAG = SQRT(DOT(R,R)); ENEG = 1.0/RMAG - 0.5*DOT(RDOT,RDOT); CROSS(R,RDOT,H); P = DOT(H,H); SQRTP = SQRT(P); for (I = 1; I <= 3; I++) { U[I] = R[I]/RMAG; W[I] = H[I]/SQRTP; } CROSS(W,U,V1); ECC = SQRT(1.0 - 2.0*ENEG*P); Q = P/(1.0 + ECC); RDU = DOT(RDOT,U); RDV = DOT(RDOT,V1); ESINV = SQRTP*RDU; ECOSV = SQRTP*RDV - 1.0; V = ATAN2(ECOSV,ESINV); SINV = SIN(V); COSV = COS(V); S = VFN(); ARG = 2.0*KSQ*ENEG*SQR(S); ARG1 = KSQ*ECC*CUB(S); TPE = Q*S + ARG1*C(3,ARG); for (I = 1; I <= 3; I++) { P1[I] = U[I]*COSV - V1[I]*SINV; Q1[I] = U[I]*SINV + V1[I]*COSV; } INCL = ACOS(W[3]); NODE = ATAN2(-W[2],W[1]); if (NODE < 0.0) NODE = NODE + TPI; ARGP = ATAN2(Q1[3],P1[3]); if (ARGP < 0.0) ARGP = ARGP + TPI; }