diff --git a/c_code/ECEFsupergravity.h b/c_code/ECEFsupergravity.h index aabcd14..91dbc1e 100644 --- a/c_code/ECEFsupergravity.h +++ b/c_code/ECEFsupergravity.h @@ -1,48 +1,100 @@ -#ifndef ECEFsupergravity_H -#define ECEFsupergravity_H +#ifndef ECEFsupergraVity_H +#define ECEFsupergraVity_H + +// size of graVity model +#define JGM3SIZE 21 #include #include #include +#include "constants.h" +#include "vecmath.h" +#include "JGM3Constants.h" -#define GM 3.986004405e14 -#define R_e 6.378137e6 +typedef struct HarmonicMatrices { + double (*V)[JGM3SIZE+1]; + double (*W)[JGM3SIZE+1]; +} harmonicmatrices; // for propagation in ECEF coordinates -double*** VW(int size, double u[]) { - // Zonal terms +harmonicmatrices* VW(double u[]) { + + harmonicmatrices* mats = (harmonicmatrices*) malloc(sizeof(harmonicmatrices)); - double r2 = u[0]*u[0] + u[1]*u[1] + u[2]*u[2]; + // Variables to make it easier double x = u[0]; double y = u[1]; double z = u[2]; + double r2 = x*x + y*y + z*z; // square of magnitude of position - double** v = (double**) malloc(size * sizeof(double*)); - double** w = (double**) malloc(size * sizeof(double*)); - - for (int i = 0; i < size; i++) { - v[i] = (double*) malloc(size * sizeof(double)); - w[i] = (double*) malloc(size * sizeof(double)); + // Initialize arrays + double (*V)[JGM3SIZE+1] = (double (*)[JGM3SIZE+1]) malloc((JGM3SIZE+1) * sizeof(double[JGM3SIZE+1])); + double (*W)[JGM3SIZE+1] = (double (*)[JGM3SIZE+1]) malloc((JGM3SIZE+1) * sizeof(double[JGM3SIZE+1])); + + W[0][0] = 0; + V[0][0] = R_e/sqrt(r2); + + // Step 1: fill in Vertical column 0 + for (int n = 1; n < JGM3SIZE+1; n++) { + // eq. 3.30 + V[n][0] = (2.0*n-1)/((double)(n)) * z * R_e/r2 * V[n-1][0] - ((double)(n-1))/((double)(n)) * R_e * R_e/r2 * V[n-2][0]; + W[n][0] = 0; + //printf("Processed index: %d 0 %f\n", n, V[n][0]); } - w[0][0] = 0; - v[0][0] = R_e/sqrt(r2); - for (int m = 1; m < size; m++) { + // Step 2: Fill in rest of the diagonals + for (int m = 1; m < JGM3SIZE+1; m++) { // eq. 3.29 - v[m][m] = (2.0*m - 1) * (x * R_e / r2 * v[m-1][m-1] - y * R_e / r2 * w[m-1][m-1]); - w[m][m] = (2.0*m - 1) * (x * R_e / r2 * w[m-1][m-1] + y * R_e / r2 * v[m-1][m-1]); + V[m][m] = (2.0*m - 1) * (x * R_e / r2 * V[m-1][m-1] - y * R_e / r2 * W[m-1][m-1]); + W[m][m] = (2.0*m - 1) * (x * R_e / r2 * W[m-1][m-1] + y * R_e / r2 * V[m-1][m-1]); + //printf("Processed index: %d %d %f\n", m, m, V[m][m]); + + // Step 3: Fill in columns under diagonal + for (int n = m+1; n < JGM3SIZE+1; n++) { + // eq. 3.30 + V[n][m] = (2.0 *n-1)/((double)(n-m)) * z * R_e / r2 * V[n-1][m] - ((double)(n+m-1))/((double)(n-m)) * R_e * R_e / r2 * V[n-2][m]; + W[n][m] = (2.0 *n-1)/((double)(n-m)) * z * R_e / r2 * W[n-1][m] - ((double)(n+m-1))/((double)(n-m)) * R_e * R_e / r2 * W[n-2][m]; + //printf("Processed index: %d %d %f\n", n, m, V[n][m]); + } } + + mats->V = V; + mats->W = W; + return mats; - for (int m = 1; m < size; m++) { +} + +void JGM_gravity(double t, double u[6], double output[6]) { + harmonicmatrices* h = VW(u); - v[m+1][m] = (2.0 *(m+1)-1) * z * R_e / r2 * v[m][m]; - w[m+1][m] = (2.0 *(m+1)-1) * z * R_e / r2 * w[m][m]; + double (*V)[JGM3SIZE+1] = h->V; + double (*W)[JGM3SIZE+1] = h->W; - for (int n = m+2; n < size; n++) { - v[n][m] = (2.0 *n-1)/((double)(n-m)) * z * R_e / r2 * v[n-1][m] - ((double)(n+m-1))/((double)(n-m) * R_e * R_e / r2 * v[n-2][m]); - w[n][m] = (2.0 *n-1)/((double)(n-m)) * z * R_e / r2 * w[n-1][m] - ((double)(n+m-1))/((double)(n-m) * R_e * R_e / r2 * w[n-2][m]); + double accel[3]; // accelerations + + // C_n,m = CS(n,m); + // S_n,m = CS(m-1,n); + + // m = 0 accelerations + for (int n = 1; n < JGM3SIZE; n++) { + accel[0] += GM / (R_e * R_e) * (-CS[n][0] * V[n+1][1]); + accel[1] += GM / (R_e * R_e) * (-CS[n][0] * W[n+1][1]); + + accel[2] += GM / (R_e * R_e) * ((n+1) * (-CS[n][0] * V[n+1][0])); + // other term with W[n+1][0] removed since these are equally zero + } + + // m > 0 accelerations + for (int m = 1; m < JGM3SIZE; m++) { + for (int n = m; n < JGM3SIZE; n++) { + accel[0] += GM / (R_e * R_e) * 0.5 * ((-CS[n][m] * V[n+1][m+1] - CS[m-1][n] * W[n+1][m+1]) + factorials[n][m] * (CS[n][m] * V[n+1][m-1] + CS[m-1][n] * W[n+1][m-1])); + accel[1] += GM / (R_e * R_e) * 0.5 * ((-CS[n][m] * W[n+1][m+1] + CS[m-1][n] * V[n+1][m+1]) + factorials[n][m] * (-CS[n][m] * W[n+1][m-1] + CS[m-1][n] * V[n+1][m-1])); + + accel[2] += GM / (R_e * R_e) * ((n-m+1) * (-CS[n][m] * V[n+1][m] - CS[m-1][n] * W[n+1][m])); } } - + + printVec(accel); + } #endif \ No newline at end of file diff --git a/c_code/JGM3Constants.h b/c_code/JGM3Constants.h index 2410749..40c10e1 100644 --- a/c_code/JGM3Constants.h +++ b/c_code/JGM3Constants.h @@ -1,133 +1,133 @@ +// C_n,m = CS(n,m); +// S_n,m = CS(m-1,n); +const double CS[21][21] = { + {1.000000e+00, 0.000000e+00, 1.543100e-09, 2.680119e-07, + -4.494599e-07, -8.066346e-08, 2.116466e-08, 6.936989e-08, + 4.019978e-08, 1.423657e-08, -8.128915e-08, -1.646546e-08, + -2.378448e-08, 2.172109e-08, 1.443750e-08, 4.154186e-09, + 1.660440e-08, -1.427822e-08, -1.817656e-08, 7.160542e-11, + 2.759192e-09}, + {0.000000e+00, 0.000000e+00, -9.038681e-07, -2.114024e-07, + 1.481555e-07, -5.232672e-08, -4.650395e-08, 9.282314e-09, + 5.381316e-09, -2.228679e-09, -3.057129e-09, -5.097360e-09, + 1.416422e-09, -2.545587e-09, -1.089217e-10, -1.045474e-09, + 7.856272e-10, 2.522818e-10, 3.427413e-10, -1.008909e-10, + 3.216826e-10}, + {-1.082627e-03, -2.414000e-10, 1.574536e-06, 1.972013e-07, + -1.201129e-08, -7.100877e-09, 1.843134e-10, -3.061150e-09, + -8.723520e-10, -5.633921e-10, -8.989333e-10, -6.863521e-10, + 9.154575e-11, 3.005522e-10, 5.182512e-11, 3.265044e-11, + -4.271981e-11, 1.297841e-11, -4.278803e-12, -1.190759e-12, + 3.778260e-11}, + {2.532435e-06, 2.192799e-06, 3.090160e-07, 1.005589e-07, + 6.525606e-09, 3.873005e-10, -1.784491e-09, -2.636182e-10, + 9.117736e-11, 1.717309e-11, -4.622483e-11, -2.677798e-11, + 9.170517e-13, -2.960682e-12, -3.750977e-12, 1.116419e-12, + 5.250141e-12, 2.159727e-12, 1.105860e-13, -3.556436e-13, + -1.178441e-12}, + {1.619331e-06, -5.087253e-07, 7.841223e-08, 5.921574e-08, + -3.982396e-09, -1.648204e-09, -4.329182e-10, 6.397253e-12, + 1.612521e-11, -5.550919e-12, -3.122269e-12, 1.982505e-12, + 2.033249e-13, 1.214266e-12, -2.217440e-13, 8.637823e-14, + -1.205563e-14, 2.923804e-14, 1.040715e-13, 9.006136e-14, + -1.823414e-14}, + {2.277161e-07, -5.371651e-08, 1.055905e-07, -1.492615e-08, + -2.297912e-09, 4.304768e-10, -5.527712e-11, 1.053488e-11, + 8.627743e-12, 2.940313e-12, -5.515591e-13, 1.346234e-13, + 9.335408e-14, -9.061871e-15, 2.365713e-15, -2.505252e-14, + -1.590014e-14, -9.295650e-15, -3.743268e-15, 3.176649e-15, + -5.637288e-17}, + {-5.396485e-07, -5.987798e-08, 6.012099e-09, 1.182266e-09, + -3.264139e-10, -2.155771e-10, 2.213693e-12, 4.475983e-13, + 3.814766e-13, -1.846792e-13, -2.650681e-15, -3.728037e-14, + 7.899913e-15, -9.747983e-16, -3.193839e-16, 2.856094e-16, + -2.590259e-16, -1.190467e-16, 8.666599e-17, -8.340023e-17, + -8.899420e-19}, + {3.513684e-07, 2.051487e-07, 3.284490e-08, 3.528541e-09, + -5.851195e-10, 5.818486e-13, -2.490718e-11, 2.559078e-14, + 1.535338e-13, -9.856184e-16, -1.052843e-14, 1.170448e-15, + 3.701523e-16, -1.095673e-16, -9.074974e-17, 7.742869e-17, + 1.086771e-17, 4.812890e-18, 2.015619e-18, -5.594661e-18, + 1.459810e-18}, + {2.025187e-07, 1.603459e-08, 6.576542e-09, -1.946358e-10, + -3.189358e-10, -4.615173e-12, -1.839364e-12, 3.429762e-13, + -1.580332e-13, 7.441039e-15, -7.011948e-16, 2.585245e-16, + 6.136644e-17, 4.870630e-17, 1.489060e-17, 1.015964e-17, + -5.700075e-18, -2.391386e-18, 1.794927e-18, 1.965726e-19, + -1.128428e-19}, + {1.193687e-07, 9.241927e-08, 1.566874e-09, -1.217275e-09, + -7.018561e-12, -1.669737e-12, 8.296725e-13, -2.251973e-13, + 6.144394e-14, -3.676763e-15, -9.892610e-17, -1.736649e-17, + 9.242424e-18, -4.153238e-18, -6.937464e-20, 3.275583e-19, + 1.309613e-19, 1.026767e-19, -1.437566e-20, -1.268576e-20, + -6.100911e-21}, + {2.480569e-07, 5.175579e-08, -5.562846e-09, -4.195999e-11, + -4.967025e-11, -3.074283e-12, -2.597232e-13, 6.909154e-15, + 4.635314e-15, 2.330148e-15, 4.170802e-16, -1.407856e-17, + -2.790078e-19, -6.376262e-20, -1.849098e-19, 3.595115e-20, + -2.537013e-21, 4.480853e-21, 4.348241e-22, 1.197796e-21, + -1.138734e-21}, + {-2.405652e-07, 9.508428e-09, 9.542030e-10, -1.409608e-10, + -1.685257e-11, 1.489441e-12, -5.754671e-15, 1.954262e-15, + -2.924949e-16, -1.934320e-16, -4.946396e-17, 9.351706e-18, + -9.838299e-20, 1.643922e-19, -1.658377e-20, 2.905537e-21, + 4.983891e-22, 6.393876e-22, -2.294907e-22, 6.437043e-23, + 6.435154e-23}, + {1.819117e-07, -3.068001e-08, 6.380398e-10, 1.451918e-10, + -2.123815e-11, 8.279902e-13, 7.883091e-15, -4.131557e-15, + -5.708254e-16, 1.012728e-16, -1.840173e-18, 4.978700e-19, + -2.108949e-20, 2.503221e-20, 3.298844e-21, -8.660491e-23, + 6.651727e-24, 5.110031e-23, -3.635064e-23, -1.311958e-23, + 1.534228e-24}, + {2.075677e-07, -2.885131e-08, 2.275183e-09, -6.676768e-11, + -3.452537e-13, 1.074251e-12, -5.281862e-14, 3.421269e-16, + -1.113494e-16, 2.658019e-17, 4.577888e-18, -5.902637e-19, + -5.860603e-20, -2.239852e-20, -6.914977e-23, -6.472496e-23, + -2.741331e-23, 2.570941e-24, -1.074458e-24, -4.305386e-25, + -2.046569e-25}, + {-1.174174e-07, -9.997710e-09, -1.347496e-09, 9.391106e-11, + 3.104170e-13, 3.932888e-13, -1.902110e-14, 2.787457e-15, + -2.125248e-16, 1.679922e-17, 1.839624e-18, 7.273780e-20, + 4.561174e-21, 2.347631e-21, -7.142240e-22, -2.274403e-24, + -2.929523e-24, 1.242605e-25, -1.447976e-25, -3.551992e-26, + -7.473051e-28}, + {1.762727e-08, 6.108862e-09, -7.164511e-10, 1.128627e-10, + -6.013879e-12, 1.293499e-13, 2.220625e-14, 2.825477e-15, + -1.112172e-16, 3.494173e-18, 2.258283e-19, -1.828153e-21, + -6.049406e-21, -5.705023e-22, 1.404654e-23, -9.295855e-24, + 5.687404e-26, 1.057368e-26, 4.931703e-27, -1.480665e-27, + 2.400400e-29}, + {-3.119431e-08, 1.356279e-08, -6.713707e-10, -6.451812e-11, + 4.698674e-12, -9.690791e-14, 6.610666e-15, -2.378057e-16, + -4.460480e-17, -3.335458e-18, -1.316568e-19, 1.643081e-20, + 1.419788e-21, 9.260416e-23, -1.349210e-23, -1.295522e-24, + -5.943715e-25, -9.608698e-27, 3.816913e-28, -3.102988e-28, + -8.192994e-29}, + {1.071306e-07, -1.262144e-08, -4.767231e-10, 1.175560e-11, + 6.946241e-13, -9.316733e-14, -4.427290e-15, 4.858365e-16, + 4.814810e-17, 2.752709e-19, -2.449926e-20, -6.393665e-21, + 8.842755e-22, 4.178428e-23, -3.177778e-24, 1.229862e-25, + -8.535124e-26, -1.658684e-26, -1.524672e-28, -2.246909e-29, + -5.508346e-31}, + {4.421672e-08, 1.958333e-09, 3.236166e-10, -5.174199e-12, + 4.022242e-12, 3.088082e-14, 3.197551e-15, 9.009281e-17, + 2.534982e-17, -9.526323e-19, 1.741250e-20, -1.569624e-21, + -4.195542e-22, -6.629972e-24, -6.574751e-25, -2.898577e-25, + 7.555273e-27, 3.046776e-28, 3.696154e-29, 1.845778e-30, + 6.948820e-31}, + {-2.197334e-08, -3.156695e-09, 7.325272e-10, -1.192913e-11, + 9.941288e-13, 3.991921e-14, -4.220405e-16, 7.091584e-17, + 1.660451e-17, 9.233532e-20, -5.971908e-20, 1.750987e-21, + -2.066463e-23, -3.440194e-24, -1.487095e-25, -4.491878e-26, + -4.558801e-27, 5.960375e-28, 8.263952e-29, -9.155723e-31, + -1.237749e-31}, + {1.203146e-07, 3.688524e-09, 4.328972e-10, -6.303973e-12, + 2.869669e-13, -3.011115e-14, 1.539793e-15, -1.390222e-16, + 1.766707e-18, 3.471731e-19, -3.447438e-20, 8.760347e-22, + -2.271884e-23, 5.960951e-24, 1.682025e-25, -2.520877e-26, + -8.774566e-28, 2.651434e-29, 8.352807e-30, -1.878413e-31, + 4.054696e-32}}; + +// (n-m+2)!/(n-m)! up to size 21 +const double factorials[21][21] = {{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 2.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 6.0, 2.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 12.0, 6.0, 2.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 20.0, 12.0, 6.0, 2.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 30.0, 20.0, 12.0, 6.0, 2.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 42.0, 30.0, 20.0, 12.0, 6.0, 2.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 56.0, 42.0, 30.0, 20.0, 12.0, 6.0, 2.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 72.0, 56.0, 42.0, 30.0, 20.0, 12.0, 6.0, 2.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 90.0, 72.0, 56.0, 42.0, 30.0, 20.0, 12.0, 6.0, 2.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 110.0, 90.0, 72.0, 56.0, 42.0, 30.0, 20.0, 12.0, 6.0, 2.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 132.0, 110.0, 90.0, 72.0, 56.0, 42.0, 30.0, 20.0, 12.0, 6.0, 2.0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 156.0, 132.0, 110.0, 90.0, 72.0, 56.0, 42.0, 30.0, 20.0, 12.0, 6.0, 2.0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 182.0, 156.0, 132.0, 110.0, 90.0, 72.0, 56.0, 42.0, 30.0, 20.0, 12.0, 6.0, 2.0, 0, 0, 0, 0, 0, 0, 0}, {0, 210.0, 182.0, 156.0, 132.0, 110.0, 90.0, 72.0, 56.0, 42.0, 30.0, 20.0, 12.0, 6.0, 2.0, 0, 0, 0, 0, 0, 0}, {0, 240.0, 210.0, 182.0, 156.0, 132.0, 110.0, 90.0, 72.0, 56.0, 42.0, 30.0, 20.0, 12.0, 6.0, 2.0, 0, 0, 0, 0, 0}, {0, 272.0, 240.0, 210.0, 182.0, 156.0, 132.0, 110.0, 90.0, 72.0, 56.0, 42.0, 30.0, 20.0, 12.0, 6.0, 2.0, 0, 0, 0, 0}, {0, 306.0, 272.0, 240.0, 210.0, 182.0, 156.0, 132.0, 110.0, 90.0, 72.0, 56.0, 42.0, 30.0, 20.0, 12.0, 6.0, 2.0, 0, 0, 0}, {0, 342.0, 306.0, 272.0, 240.0, 210.0, 182.0, 156.0, 132.0, 110.0, 90.0, 72.0, 56.0, 42.0, 30.0, 20.0, 12.0, 6.0, 2.0, 0, 0}, {0, 380.0, 342.0, 306.0, 272.0, 240.0, 210.0, 182.0, 156.0, 132.0, 110.0, 90.0, 72.0, 56.0, 42.0, 30.0, 20.0, 12.0, 6.0, 2.0, 0}, {0, 420.0, 380.0, 342.0, 306.0, 272.0, 240.0, 210.0, 182.0, 156.0, 132.0, 110.0, 90.0, 72.0, 56.0, 42.0, 30.0, 20.0, 12.0, 6.0, 2.0}}; -namespace centralbodyforce{ - // C_n,m = CS(n,m); - // S_n,m = CS(m-1,n); - const double R = 6378.114; - const double CS[21][21] = { - {1.000000e+00, 0.000000e+00, 1.543100e-09, 2.680119e-07, - -4.494599e-07, -8.066346e-08, 2.116466e-08, 6.936989e-08, - 4.019978e-08, 1.423657e-08, -8.128915e-08, -1.646546e-08, - -2.378448e-08, 2.172109e-08, 1.443750e-08, 4.154186e-09, - 1.660440e-08, -1.427822e-08, -1.817656e-08, 7.160542e-11, - 2.759192e-09}, - {0.000000e+00, 0.000000e+00, -9.038681e-07, -2.114024e-07, - 1.481555e-07, -5.232672e-08, -4.650395e-08, 9.282314e-09, - 5.381316e-09, -2.228679e-09, -3.057129e-09, -5.097360e-09, - 1.416422e-09, -2.545587e-09, -1.089217e-10, -1.045474e-09, - 7.856272e-10, 2.522818e-10, 3.427413e-10, -1.008909e-10, - 3.216826e-10}, - {-1.082627e-03, -2.414000e-10, 1.574536e-06, 1.972013e-07, - -1.201129e-08, -7.100877e-09, 1.843134e-10, -3.061150e-09, - -8.723520e-10, -5.633921e-10, -8.989333e-10, -6.863521e-10, - 9.154575e-11, 3.005522e-10, 5.182512e-11, 3.265044e-11, - -4.271981e-11, 1.297841e-11, -4.278803e-12, -1.190759e-12, - 3.778260e-11}, - {2.532435e-06, 2.192799e-06, 3.090160e-07, 1.005589e-07, - 6.525606e-09, 3.873005e-10, -1.784491e-09, -2.636182e-10, - 9.117736e-11, 1.717309e-11, -4.622483e-11, -2.677798e-11, - 9.170517e-13, -2.960682e-12, -3.750977e-12, 1.116419e-12, - 5.250141e-12, 2.159727e-12, 1.105860e-13, -3.556436e-13, - -1.178441e-12}, - {1.619331e-06, -5.087253e-07, 7.841223e-08, 5.921574e-08, - -3.982396e-09, -1.648204e-09, -4.329182e-10, 6.397253e-12, - 1.612521e-11, -5.550919e-12, -3.122269e-12, 1.982505e-12, - 2.033249e-13, 1.214266e-12, -2.217440e-13, 8.637823e-14, - -1.205563e-14, 2.923804e-14, 1.040715e-13, 9.006136e-14, - -1.823414e-14}, - {2.277161e-07, -5.371651e-08, 1.055905e-07, -1.492615e-08, - -2.297912e-09, 4.304768e-10, -5.527712e-11, 1.053488e-11, - 8.627743e-12, 2.940313e-12, -5.515591e-13, 1.346234e-13, - 9.335408e-14, -9.061871e-15, 2.365713e-15, -2.505252e-14, - -1.590014e-14, -9.295650e-15, -3.743268e-15, 3.176649e-15, - -5.637288e-17}, - {-5.396485e-07, -5.987798e-08, 6.012099e-09, 1.182266e-09, - -3.264139e-10, -2.155771e-10, 2.213693e-12, 4.475983e-13, - 3.814766e-13, -1.846792e-13, -2.650681e-15, -3.728037e-14, - 7.899913e-15, -9.747983e-16, -3.193839e-16, 2.856094e-16, - -2.590259e-16, -1.190467e-16, 8.666599e-17, -8.340023e-17, - -8.899420e-19}, - {3.513684e-07, 2.051487e-07, 3.284490e-08, 3.528541e-09, - -5.851195e-10, 5.818486e-13, -2.490718e-11, 2.559078e-14, - 1.535338e-13, -9.856184e-16, -1.052843e-14, 1.170448e-15, - 3.701523e-16, -1.095673e-16, -9.074974e-17, 7.742869e-17, - 1.086771e-17, 4.812890e-18, 2.015619e-18, -5.594661e-18, - 1.459810e-18}, - {2.025187e-07, 1.603459e-08, 6.576542e-09, -1.946358e-10, - -3.189358e-10, -4.615173e-12, -1.839364e-12, 3.429762e-13, - -1.580332e-13, 7.441039e-15, -7.011948e-16, 2.585245e-16, - 6.136644e-17, 4.870630e-17, 1.489060e-17, 1.015964e-17, - -5.700075e-18, -2.391386e-18, 1.794927e-18, 1.965726e-19, - -1.128428e-19}, - {1.193687e-07, 9.241927e-08, 1.566874e-09, -1.217275e-09, - -7.018561e-12, -1.669737e-12, 8.296725e-13, -2.251973e-13, - 6.144394e-14, -3.676763e-15, -9.892610e-17, -1.736649e-17, - 9.242424e-18, -4.153238e-18, -6.937464e-20, 3.275583e-19, - 1.309613e-19, 1.026767e-19, -1.437566e-20, -1.268576e-20, - -6.100911e-21}, - {2.480569e-07, 5.175579e-08, -5.562846e-09, -4.195999e-11, - -4.967025e-11, -3.074283e-12, -2.597232e-13, 6.909154e-15, - 4.635314e-15, 2.330148e-15, 4.170802e-16, -1.407856e-17, - -2.790078e-19, -6.376262e-20, -1.849098e-19, 3.595115e-20, - -2.537013e-21, 4.480853e-21, 4.348241e-22, 1.197796e-21, - -1.138734e-21}, - {-2.405652e-07, 9.508428e-09, 9.542030e-10, -1.409608e-10, - -1.685257e-11, 1.489441e-12, -5.754671e-15, 1.954262e-15, - -2.924949e-16, -1.934320e-16, -4.946396e-17, 9.351706e-18, - -9.838299e-20, 1.643922e-19, -1.658377e-20, 2.905537e-21, - 4.983891e-22, 6.393876e-22, -2.294907e-22, 6.437043e-23, - 6.435154e-23}, - {1.819117e-07, -3.068001e-08, 6.380398e-10, 1.451918e-10, - -2.123815e-11, 8.279902e-13, 7.883091e-15, -4.131557e-15, - -5.708254e-16, 1.012728e-16, -1.840173e-18, 4.978700e-19, - -2.108949e-20, 2.503221e-20, 3.298844e-21, -8.660491e-23, - 6.651727e-24, 5.110031e-23, -3.635064e-23, -1.311958e-23, - 1.534228e-24}, - {2.075677e-07, -2.885131e-08, 2.275183e-09, -6.676768e-11, - -3.452537e-13, 1.074251e-12, -5.281862e-14, 3.421269e-16, - -1.113494e-16, 2.658019e-17, 4.577888e-18, -5.902637e-19, - -5.860603e-20, -2.239852e-20, -6.914977e-23, -6.472496e-23, - -2.741331e-23, 2.570941e-24, -1.074458e-24, -4.305386e-25, - -2.046569e-25}, - {-1.174174e-07, -9.997710e-09, -1.347496e-09, 9.391106e-11, - 3.104170e-13, 3.932888e-13, -1.902110e-14, 2.787457e-15, - -2.125248e-16, 1.679922e-17, 1.839624e-18, 7.273780e-20, - 4.561174e-21, 2.347631e-21, -7.142240e-22, -2.274403e-24, - -2.929523e-24, 1.242605e-25, -1.447976e-25, -3.551992e-26, - -7.473051e-28}, - {1.762727e-08, 6.108862e-09, -7.164511e-10, 1.128627e-10, - -6.013879e-12, 1.293499e-13, 2.220625e-14, 2.825477e-15, - -1.112172e-16, 3.494173e-18, 2.258283e-19, -1.828153e-21, - -6.049406e-21, -5.705023e-22, 1.404654e-23, -9.295855e-24, - 5.687404e-26, 1.057368e-26, 4.931703e-27, -1.480665e-27, - 2.400400e-29}, - {-3.119431e-08, 1.356279e-08, -6.713707e-10, -6.451812e-11, - 4.698674e-12, -9.690791e-14, 6.610666e-15, -2.378057e-16, - -4.460480e-17, -3.335458e-18, -1.316568e-19, 1.643081e-20, - 1.419788e-21, 9.260416e-23, -1.349210e-23, -1.295522e-24, - -5.943715e-25, -9.608698e-27, 3.816913e-28, -3.102988e-28, - -8.192994e-29}, - {1.071306e-07, -1.262144e-08, -4.767231e-10, 1.175560e-11, - 6.946241e-13, -9.316733e-14, -4.427290e-15, 4.858365e-16, - 4.814810e-17, 2.752709e-19, -2.449926e-20, -6.393665e-21, - 8.842755e-22, 4.178428e-23, -3.177778e-24, 1.229862e-25, - -8.535124e-26, -1.658684e-26, -1.524672e-28, -2.246909e-29, - -5.508346e-31}, - {4.421672e-08, 1.958333e-09, 3.236166e-10, -5.174199e-12, - 4.022242e-12, 3.088082e-14, 3.197551e-15, 9.009281e-17, - 2.534982e-17, -9.526323e-19, 1.741250e-20, -1.569624e-21, - -4.195542e-22, -6.629972e-24, -6.574751e-25, -2.898577e-25, - 7.555273e-27, 3.046776e-28, 3.696154e-29, 1.845778e-30, - 6.948820e-31}, - {-2.197334e-08, -3.156695e-09, 7.325272e-10, -1.192913e-11, - 9.941288e-13, 3.991921e-14, -4.220405e-16, 7.091584e-17, - 1.660451e-17, 9.233532e-20, -5.971908e-20, 1.750987e-21, - -2.066463e-23, -3.440194e-24, -1.487095e-25, -4.491878e-26, - -4.558801e-27, 5.960375e-28, 8.263952e-29, -9.155723e-31, - -1.237749e-31}, - {1.203146e-07, 3.688524e-09, 4.328972e-10, -6.303973e-12, - 2.869669e-13, -3.011115e-14, 1.539793e-15, -1.390222e-16, - 1.766707e-18, 3.471731e-19, -3.447438e-20, 8.760347e-22, - -2.271884e-23, 5.960951e-24, 1.682025e-25, -2.520877e-26, - -8.774566e-28, 2.651434e-29, 8.352807e-30, -1.878413e-31, - 4.054696e-32}}; -} \ No newline at end of file diff --git a/c_code/constants.h b/c_code/constants.h new file mode 100644 index 0000000..1d5b3a8 --- /dev/null +++ b/c_code/constants.h @@ -0,0 +1,16 @@ +// Constants used by Supernova + +#ifndef CONSTANTS_H +#define CONSTANTS_H + +#define GM 3.986004405e14 +// GM in SI metre units +#define R_e 6.378137e6 +// Radius of Earth in m +#define J2 1.08262668e-3 +// J2 in SI metre units +#define W_e 7.2921159e-5 +// Earth's angular velocity in rad/s +#define PI 3.14159265358979323846 + +#endif \ No newline at end of file diff --git a/c_code/main.c b/c_code/main.c index dbc547a..129fb71 100644 --- a/c_code/main.c +++ b/c_code/main.c @@ -3,8 +3,8 @@ #include "orbit.h" int main(void) { - double y0[6] = {(6378 + 500)*1e3, 0, 0, 0, 7.5e3, 0}; - double tSpan[2] = {0, 1000000}; + double y0[6] = {(6378 + 500)*1e3, 0, 0, 0, 7.6e3, 0}; + double tSpan[2] = {0, 86400*10}; double tol = 1e-6; orbit("RK810", tSpan, y0, "RK810.csv", tol); orbit("RK1012", tSpan, y0, "RK1012.csv", tol); diff --git a/c_code/makefile b/c_code/makefile index d4690df..32b29f0 100644 --- a/c_code/makefile +++ b/c_code/makefile @@ -11,4 +11,4 @@ python: gcc -fPIC -shared -o ../python_code/supernova.so main.c -O3 -lm -fopt-info-vec test: - gcc -o main testvec.c -Wall -lm \ No newline at end of file + gcc -o main testgravity.c -Wall -lm \ No newline at end of file diff --git a/c_code/orbit.h b/c_code/orbit.h index 6be4b72..2162e3b 100644 --- a/c_code/orbit.h +++ b/c_code/orbit.h @@ -50,13 +50,12 @@ solution* orbitPTR(char solver[], double tSpan[], double y0[], double ATOL) { y0: initial x y z vx vy vz state vector ATOL: tolerance */ - solution* result; - if (!strcmp(solver, "RK810")) result = RK810vec(combined_perturbations, tSpan, y0, ATOL); - else if (!strcmp(solver, "RK1012")) result = RK1012vec(combined_perturbations, tSpan, y0, ATOL); + if (!strcmp(solver, "RK810")) return RK810vec(combined_perturbations, tSpan, y0, ATOL); + else if (!strcmp(solver, "RK1012")) return RK1012vec(combined_perturbations, tSpan, y0, ATOL); else { printf("Invalid solver chosen.\n"); + return NULL; } - return result; } #endif \ No newline at end of file diff --git a/c_code/physics.h b/c_code/physics.h index 1ae0567..82b58ac 100644 --- a/c_code/physics.h +++ b/c_code/physics.h @@ -1,15 +1,42 @@ #ifndef PHYSICS_H #define PHYSICS_H -#define GM 3.986004405e14 -#define R_e 6.378137e6 -#define J2 1.08262668e-3 +// Aero configuration (static in code for now) +#define CD 2.2 +#define Am 0.01 + +// Model configurations +#define J2_ON 1 +#define AERO_ON 1 + #include +#include "constants.h" #include "vecmath.h" // Physics models // operates on 6D state vector -const double earthomega[3] = {0, 0, 72.9211e-6}; + +double atmosphere(double z) { + // Atmospheric density based on Curtis D.41 + double h[] = {0, 25e3, 30e3, 40e3, 50e3, 60e3, 70e3, 80e3, 90e3, 100e3, 110e3, 120e3, 130e3, 140e3, 150e3, 180e3, 200e3, 250e3, 300e3, 350e3, 400e3, 450e3, 500e3, 600e3, 700e3, 800e3, 900e3, 1000}; + // geometric heights in m + + double r[] = {1.225, 4.008e-2, 1.841e-2, 3.996e-3, 1.027e-3, 3.097e-4, 8.283e-5, 1.846e-5, 3.416e-6, 5.606e-7, 9.708e-8, 2.222e-8, 8.152e-9, 3.831e-9, 2.076e-9, 5.194e-10, 2.541e-10, 6.073e-11, 1.916e-11, 7.014e-12, 2.803e-12, 1.184e-12, 5.215e-13, 1.137e-13, 3.070e-14, 1.136e-14, 5.759e-15, 3.561e-15}; + // densities in kg/m^3 + + double H[] = {7.310e3, 6.427e3, 6.546e3, 7.360e3, 8.342e3, 7.583e3, 6.661e3, 5.927e3, 5.533e3, 5.703e3, 6.782e3, 9.973e3, 13.243e3, 16.322e3, 21.652e3, 27.974e3, 34.934e3, 43.342e3, 49.755e3, 54.513e3, 58.019e3, 60.980e3, 65.654e3, 76.377e3, 100.587e3, 147.203e3, 208.020e3, 0}; + // scale heights in m + + if (z < 0) z = 0; + else if (z > 1000e3) z = 1000e3; + + int i; + for (i = 0; i < 27; i++) { + if (z >= h[i] && z < h[i+1]) break; // we found the correct altitude + } + + return r[i] * exp(-(z-h[i])/H[i]); +} void combined_perturbations(double t, double u[6], double output[6]) { /* @@ -18,12 +45,8 @@ void combined_perturbations(double t, double u[6], double output[6]) { u: vector of size 6 containing x y z vx vy vz output: vector of size m which stores x' y' z' vx' vy' vz' */ - double mag_r = sqrt(u[0]*u[0] + u[1]*u[1] + u[2]*u[2]); - double factor = (3.0/2.0) * GM * R_e * R_e * J2 / pow(mag_r, 5); - //double mag_v = sqrt(u[3]*u[3] + u[4]*u[4] + u[5]*u[5]); - - //double atmvel[3]; - //cross(u, earthomega, atmvel); + double mag_r = sqrt(u[0]*u[0] + u[1]*u[1] + u[2]*u[2]); // distance of spacecraft from centre of Earth + double factor = (3.0/2.0) * GM * R_e * R_e * J2 / pow(mag_r, 5); // J2 Factor // 0. Velocity update for (int i = 0; i < 3; i++) output[i] = u[i+3]; @@ -31,15 +54,34 @@ void combined_perturbations(double t, double u[6], double output[6]) { // 1. Earth Gravity for (int i = 0; i < 3; i++) output[i+3] = -GM * u[i] / (mag_r * mag_r * mag_r); // v' = -GM r / |r|^3 + #if J2_ON == 1 // 2. J2 effect // J2 Perturbation, from Curtis 12.30 output[3] += factor * u[0] * (5 * u[2] * u[2] / (mag_r * mag_r) - 1); output[4] += factor * u[1] * (5 * u[2] * u[2] / (mag_r * mag_r) - 1); output[5] += factor * u[2] * (5 * u[2] * u[2] / (mag_r * mag_r) - 3); + #endif + + #if AERO_ON == 1 // 3. Atmospheric Drag - // double rho = 1; - // for (int i = 3; i < 6; i++) output[i] -= rho * CD * AM * u[i] * mag_v/2.0; + // from Curtis 12.12 + + double v_rel[3] = {u[3]+u[1]*W_e, u[4]-W_e*u[0], u[5]}; + // determine atmospheric velocity and subsequent relative velocity by subtracting atmospheric velocity + + //printVec(v_rel); + + double mag_v_rel = sqrt(v_rel[0]*v_rel[0] + v_rel[1]*v_rel[1] + v_rel[2]*v_rel[2]); // magnitude of relative velocity + + double rho = atmosphere(mag_r-R_e); // atmospheric density at altitude + + for (int i = 3; i < 6; i++) { + output[i] -= 0.5 * rho * CD * Am * mag_v_rel * v_rel[i-3]; + //printf("Acceleration from drag in direction %d: %.10f m/s^2\t", i-3, -0.5 * rho * CD * Am * mag_v_rel * v_rel[i-3]); + } + #endif + return; } diff --git a/c_code/testgravity.c b/c_code/testgravity.c new file mode 100644 index 0000000..21b61ef --- /dev/null +++ b/c_code/testgravity.c @@ -0,0 +1,11 @@ +#include +#include +#include +#include "ECEFsupergravity.h" + +int main(void) { + + double u[6] = {5000e3, 0, 1000e3, 0, 0, 0}; + JGM_gravity(1.0, u, NULL); + +} \ No newline at end of file diff --git a/c_code/vecmath.h b/c_code/vecmath.h index ca77b48..b7ac71b 100644 --- a/c_code/vecmath.h +++ b/c_code/vecmath.h @@ -1,11 +1,10 @@ #ifndef VECMATH_H #define VECMATH_H -// vector math library +// Vector Math Utilities for Supernova #include #include -#define PI 3.14159265358979323846 -#define GM 3.986004405e14 +#include "constants.h" void cross(double A[3], double B[3], double U[3]) { // Crosses vectors A and B then stores result in vector U @@ -45,6 +44,9 @@ void transpose(double (*A)[3]) { } } +/* +Specialty Matrices +*/ double (*QXp(double aop, double inc, double raan))[3] { // ECI to Perifocal matrix @@ -90,6 +92,46 @@ double (*QpX(double aop, double inc, double raan))[3] { return matrix; } +void ECI2ECEF(double jd, double (*matrix)[3]) { + // ECI to ECEF matrix, given time in Julian + + // Get Earth rotation angle given time using Celest algorithm. + // https://github.com/JaiWillems/Celest/blob/develop-v0.2.0/celest/satellite/coordinate.py + double ERA = -fmod(6.30038748702467 * (jd - 2451545) + 4.895, 2*PI); + + matrix[0][0] = cos(ERA); + matrix[0][1] = -sin(ERA); + matrix[0][2] = 0; + + matrix[1][0] = sin(ERA); + matrix[1][1] = cos(ERA); + matrix[1][2] = 0; + + matrix[2][0] = 0; + matrix[2][1] = 0; + matrix[2][2] = 0; + +} + +void ECEF2ECI(double jd, double (*matrix)[3]) { + // ECEF to ECI matrix, given time in Julian + + double ERA = fmod(6.30038748702467 * (jd - 2451545) + 4.895, 2*PI); + + matrix[0][0] = cos(ERA); + matrix[0][1] = -sin(ERA); + matrix[0][2] = 0; + + matrix[1][0] = sin(ERA); + matrix[1][1] = cos(ERA); + matrix[1][2] = 0; + + matrix[2][0] = 0; + matrix[2][1] = 0; + matrix[2][2] = 0; + +} + double* PFE(double a, double e, double i, double aop, double m) { // Returns perifocal position and velocity as 6-vector diff --git a/python_code/plotter.py b/python_code/plotter.py index c7cec2e..52f12fa 100644 --- a/python_code/plotter.py +++ b/python_code/plotter.py @@ -58,4 +58,4 @@ def plot_error(diff: np.ndarray): if __name__ == "__main__": - plot_trajectory("c_code/RKF56.csv", "c_code/RK45.csv", "c_code/RK810.csv") + plot_trajectory("c_code/RK810.csv", "c_code/RK1012.csv")