Skip to content

Commit

Permalink
added geodetic partials
Browse files Browse the repository at this point in the history
  • Loading branch information
@rahil-makadia
rahil-makadia committed Oct 29, 2024
1 parent 19a027f commit dbacf2c
Show file tree
Hide file tree
Showing 5 changed files with 123 additions and 26 deletions.
2 changes: 2 additions & 0 deletions include/simulation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -388,6 +388,8 @@ class ImpactParameters : public CloseApproachParameters {
real lon;
real lat;
real alt;
std::vector<real> dlon = std::vector<real>(6, std::numeric_limits<real>::quiet_NaN());
std::vector<real> dlat = std::vector<real>(6, std::numeric_limits<real>::quiet_NaN());
/**
* @brief Get the impact parameters.
*/
Expand Down
10 changes: 10 additions & 0 deletions include/utilities.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,16 @@ using std::tanh;
*
*/
#define EARTH_OBLIQUITY 84381.448 / 3600.0 * DEG2RAD
/**
* @brief Value of Earth's WGS84 radius in AU.
*
*/
#define EARTH_RAD_WGS84 6378.137 / 1.495978707e8
/**
* @brief Value of Earth's WGS84 flattening.
*
*/
#define EARTH_FLAT_WGS84 1.0 / 298.257223563

/**
* @brief Wrap an angle to the range [0, 2*pi).
Expand Down
123 changes: 100 additions & 23 deletions src/approach.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,28 +27,79 @@ static real get_atm_offset(const int &centralBodySpiceId){
* @param[in] x X-coordinate in body-fixed frame.
* @param[in] y Y-coordinate in body-fixed frame.
* @param[in] z Z-coordinate in body-fixed frame.
* @param[in] a Semi-major axis of the body.
* @param[in] f Flattening of the body.
* @param[out] lon Longitude in geodetic frame.
* @param[out] lat Latitude in geodetic frame.
* @param[out] h Height above spheroid in geodetic frame.
*/
static void rec_to_geodetic(const real &x, const real &y, const real &z,
const real &a, const real &f, real &lon, real &lat,
real &h) {
real &lon, real &lat, real &h) {
const real a = EARTH_RAD_WGS84;
const real f = EARTH_FLAT_WGS84;
if (x == 0.0 && y == 0.0) {
lon = 0.0;
} else {
lon = atan2(y, x);
if (lon < 0.0) {
lon += 2 * PI;
}
}
const real xy = sqrt(x*x + y*y);
const real e2 = 2*f - f*f;
real phi = atan(z/(xy*(1-e2)));
if (xy == 0.0) {
if (z >= 0.0) {
lat = PI/2;
} else {
lat = -PI/2;
}
h = fabs(z) - a*sqrt(1-e2);
return;
}
lat = atan(z/(xy*(1-e2)));
real N;
for (size_t k = 0; k < 5; k++) {
N = a/sqrt(1-e2*sin(phi)*sin(phi));
h = xy/cos(phi) - N;
phi = atan(z*(N+h)/(xy*(N*(1-e2)+h)));
N = a/sqrt(1-e2*sin(lat)*sin(lat));
h = xy/cos(lat) - N;
lat = atan(z*(N+h)/(xy*(N*(1-e2)+h)));
}
lat = phi;
lon = atan2(y, x);
if (lon < 0.0) {
lon += 2 * PI;
}

static void rec_to_geodetic_jac(const real &lon, const real &lat, const real &h,
std::vector< std::vector<real> > &jac) {
// calculate derivatives of geodetic coordinates with respect to the
// rectangular coordinates by inversion of the derivatives of the
// rectangular coordinates with respect to the geodetic coordinates
const real a = EARTH_RAD_WGS84;
const real slat = sin(lat);
const real clat = cos(lat);
const real slon = sin(lon);
const real clon = cos(lon);
const real flat = 1.0 - EARTH_FLAT_WGS84;
const real flat2 = flat * flat;
const real g = sqrt(clat*clat + flat2*slat*slat);
const real g2 = g * g;
const real dg_dlat = (-1.0 + flat2) * slat * clat / g;
std::vector< std::vector<real> > jacInv = std::vector< std::vector<real> >(3, std::vector<real>(3, 0.0));
// partials of rectangular coordinates w.r.t. geodetic longitude
jacInv[0][0] = - (h + a/g) * slon * clat;
jacInv[1][0] = (h + a/g) * clon * clat;
jacInv[2][0] = 0.0;
// partials of rectangular coordinates w.r.t. geodetic latitude
jacInv[0][1] = (-a*dg_dlat/g2) * clon * clat - (h + a/g) * clon * slat;
jacInv[1][1] = (-a*dg_dlat/g2) * slon * clat - (h + a/g) * slon * slat;
jacInv[2][1] = (-flat2*a*dg_dlat/g2) * slat + (h + flat2*a/g) * clat;
// partials of rectangular coordinates w.r.t. geodetic altitude
jacInv[0][2] = clon * clat;
jacInv[1][2] = slon * clat;
jacInv[2][2] = slat;
// invert the Jacobian
std::vector< std::vector<real> > jacSmall = std::vector< std::vector<real> >(3, std::vector<real>(3, 0.0));
mat3_inv(jacInv, jacSmall);
// fill the full 2x6 Jacobian for just longitude and latitude
jac.resize(2, std::vector<real>(6, 0.0));
for (size_t i = 0; i < 2; i++) {
for (size_t j = 0; j < 3; j++) {
jac[i][j] = jacSmall[i][j];
}
}
}

Expand Down Expand Up @@ -204,10 +255,8 @@ void impact_r_calc(PropSimulation *propSim, const size_t &i, const size_t &j,
const real x = xBody[0];
const real y = xBody[1];
const real z = xBody[2];
const real a = 6378137.0/propSim->consts.du2m;
const real f = 1/298.257223563;
real lon, lat, h;
rec_to_geodetic(x, y, z, a, f, lon, lat, h);
rec_to_geodetic(x, y, z, lon, lat, h);
r = h - atmOffset;
return;
} else {
Expand Down Expand Up @@ -981,11 +1030,10 @@ void ImpactParameters::get_impact_parameters(PropSimulation *propSim){
x = this->xRelBodyFixed[0];
y = this->xRelBodyFixed[1];
z = this->xRelBodyFixed[2];
std::vector<std::vector<real>> jac(2, std::vector<real>(6, 0.0));
if (this->centralBodySpiceId == 399){
const real a = 6378137.0/propSim->consts.du2m;
const real f = 1/298.257223563;
rec_to_geodetic(x, y, z, a, f, lon, lat, alt);
alt *= propSim->consts.du2m/1.0e3L;
rec_to_geodetic(x, y, z, lon, lat, alt);
rec_to_geodetic_jac(lon, lat, alt, jac);
} else {
const real dist = sqrt(x*x + y*y + z*z);
lat = atan2(z, sqrt(x*x + y*y));
Expand All @@ -999,11 +1047,40 @@ void ImpactParameters::get_impact_parameters(PropSimulation *propSim){
} else {
centralBodyRadius = propSim->spiceBodies[this->centralBodyIdx - propSim->integParams.nInteg].radius;
}
alt = (dist-centralBodyRadius)*propSim->consts.du2m/1.0e3L;
alt = dist-centralBodyRadius;
// calculate derivatives of latitudinal coordinates with respect to the
// rectangular coordinates by inversion of the derivatives of the
// rectangular coordinates with respect to the latitudinal coordinates
std::vector<std::vector<real>> jacInv(3, std::vector<real>(3, 0.0));
// partials of rectangular coordinates with respect to longitude
jacInv[0][1] = -dist * sin(lon) * cos(lat);
jacInv[1][1] = dist * cos(lon) * cos(lat);
jacInv[2][1] = 0.0;
// partials of rectangular coordinates with respect to latitude
jacInv[0][2] = -dist * cos(lon) * sin(lat);
jacInv[1][2] = -dist * sin(lon) * sin(lat);
jacInv[2][2] = dist * cos(lat);
// partials of rectangular coordinates with respect to radius
jacInv[0][3] = cos(lon) * cos(lat);
jacInv[1][3] = sin(lon) * cos(lat);
jacInv[2][3] = sin(lat);
std::vector<std::vector<real>> jacSmall(3, std::vector<real>(3, 0.0));
mat3_inv(jacInv, jacSmall);
for (size_t k = 0; k < 2; k++) {
for (size_t k2 = 0; k2 < 3; k2++) {
jac[k][k2] = jacSmall[k][k2];
}
}
}
this->lon = lon;
this->lat = lat;
this->alt = alt;
std::vector<std::vector<real>> jac_inertial(2, std::vector<real>(6, 0.0));
mat_mat_mul(jac, rotMat, jac_inertial);
for (size_t k = 0; k < 6; k++) {
this->dlon[k] = jac_inertial[0][k];
this->dlat[k] = jac_inertial[1][k];
}
}

/**
Expand All @@ -1014,7 +1091,7 @@ void ImpactParameters::print_summary(int prec){
std::cout << "MJD " << this->t << " TDB:" << std::endl;
std::cout << " " << this->flybyBody << " impacted " << this->centralBody << " with a relative velocity of " << this->vel << " AU/d." << std::endl;
std::cout << " Impact location: " << std::endl;
std::cout << " Longitude: " << this->lon*180.0L/PI << " deg" << std::endl;
std::cout << " Latitude: " << this->lat*180.0L/PI << " deg" << std::endl;
std::cout << " Altitude: " << this->alt << " km" << std::endl;
std::cout << " Longitude: " << this->lon*RAD2DEG << " deg" << std::endl;
std::cout << " Latitude: " << this->lat*RAD2DEG << " deg" << std::endl;
std::cout << " Altitude: " << this->alt*1.495978707e8 << " km" << std::endl;
}
6 changes: 6 additions & 0 deletions src/grss.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -309,6 +309,12 @@ PYBIND11_MODULE(libgrss, m) {
.def_readwrite("alt", &ImpactParameters::alt, R"mydelimiter(
Altitude of the impact.
)mydelimiter")
.def_readwrite("dlon", &ImpactParameters::dlon, R"mydelimiter(
Partial derivatives of the longitude of the impact with respect to the state at the impact.
)mydelimiter")
.def_readwrite("dlat", &ImpactParameters::dlat, R"mydelimiter(
Partial derivatives of the latitude of the impact with respect to the state at the impact.
)mydelimiter")
.def("print_summary", &ImpactParameters::print_summary,
py::arg("prec") = 8, R"mydelimiter(
Print a summary of the impact parameters.
Expand Down
8 changes: 5 additions & 3 deletions src/simulation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -130,6 +130,8 @@ void Body::set_J2(real J2, real poleRA, real poleDec) {
/**
* @param[in] poleRA Right ascension of the pole.
* @param[in] poleDec Declination of the pole.
* @param[in] nZon Degree of the zonal coefficients.
* @param[in] nTes Order of the tesseral coefficients.
* @param[in] J Zonal coefficient vector.
* @param[in] C Sectoral coefficient array.
* @param[in] S Tesseral coefficient array.
Expand Down Expand Up @@ -1515,9 +1517,9 @@ void PropSimulation::save(std::string filename) {
file << "MJD " << timeWidth << std::fixed << imp.t << " TDB:" << std::endl;
file << " " << imp.flybyBody << " impacted " << imp.centralBody << " with a relative velocity of " << imp.vel << " AU/d." << std::endl;
file << " Impact location: " << std::endl;
file << " Longitude: " << imp.lon*180.0L/PI << " deg" << std::endl;
file << " Latitude: " << imp.lat*180.0L/PI << " deg" << std::endl;
file << " Altitude: " << imp.alt << " km" << std::endl;
file << " Longitude: " << imp.lon*RAD2DEG << " deg" << std::endl;
file << " Latitude: " << imp.lat*RAD2DEG << " deg" << std::endl;
file << " Altitude: " << imp.alt*this->consts.du2m/1.0e3 << " km" << std::endl;
}
}

Expand Down

0 comments on commit dbacf2c

Please sign in to comment.