Updates to USStandardAtmosphere

src/sim/CMakeLists.txt

@@ -2,6 +2,8 @@ add_library(sim
    AtmosphericModel.cpp
    GravityModel.cpp
    WindModel.cpp
-   USStandardAtmosphere.cpp)
+   USStandardAtmosphere.cpp
+   SphericalGravityModel.cpp
+   SphericalGeoidModel.cpp)
 
-target_link_libraries(sim PRIVATE math)
+target_link_libraries(sim PRIVATE math utils)

src/sim/GeoidModel.h

@@ -0,0 +1,25 @@
+#ifndef SIM_GEOIDMODEL_H
+#define SIM_GEOIDMODEL_H
+
+namespace sim
+{
+
+/**
+ * @brief The GeoidModel represents the physical ellipsoid of the earth. It is used
+ *        to determin the distance of the local ground level from the center of mass
+ *        of the earth.
+ * 
+ */
+class GeoidModel
+{
+public:
+   GeoidModel() {}
+   virtual ~GeoidModel() {}
+
+   virtual double getGroundLevel(double latitude, double longitude) = 0;
+
+};
+
+} // namespace sim
+
+#endif // SIM_GEOIDMODEL_H

src/sim/SphericalGeoidModel.cpp

@@ -0,0 +1,21 @@
+#include "SphericalGeoidModel.h"
+#include "utils/math/Constants.h"
+
+namespace sim
+{
+
+SphericalGeoidModel::SphericalGeoidModel()
+{
+}
+
+SphericalGeoidModel::~SphericalGeoidModel()
+{
+
+}
+
+double SphericalGeoidModel::getGroundLevel(double, double)
+{
+   return  utils::math::Constants::meanEarthRadiusWGS84;
+}
+
+} // namespace sim

src/sim/SphericalGeoidModel.h

@@ -0,0 +1,27 @@
+#ifndef SIM_SPHERICALGEOIDMODEL_H
+#define SIM_SPHERICALGEOIDMODEL_H
+
+#include "GeoidModel.h"
+
+namespace sim
+{
+
+/**
+ * @brief The SphericalGeoidModel returns the average of the polar radius and equatorial
+ *        radius of the Earth, based on WGS84
+ * 
+ */
+
+class SphericalGeoidModel : public GeoidModel
+{
+public:
+   SphericalGeoidModel();
+   virtual ~SphericalGeoidModel();
+
+   double getGroundLevel(double latitude, double longitude) override;
+
+};
+
+} // namespace sim
+
+#endif // SIM_SPHERICALGEOIDMODEL_H

src/sim/SphericalGravityModel.cpp

@@ -0,0 +1,43 @@
+#include "SphericalGravityModel.h"
+
+#include "utils/math/Constants.h"
+
+#include <cmath>
+#include <tuple>
+
+namespace sim
+{
+
+SphericalGravityModel::SphericalGravityModel()
+{
+
+}
+
+SphericalGravityModel::~SphericalGravityModel()
+{
+
+}
+
+std::tuple<double, double, double> SphericalGravityModel::getAccel(double x, double y, double z)
+{
+   // Convert x, y, z from meters to km. This is to avoid potential precision losses
+   // with using the earth's gravitation parameter in meters (14 digit number).
+   // GM in kilometers is much more manageable.
+   // An alternative is to use quadruple precision, but that may
+   // take a lot more computation time and I think this will be fine.
+   double x_km = x / 1000.0;
+   double y_km = y / 1000.0;
+   double z_km = z / 1000.0;
+
+   double r_km = std::sqrt(x_km * x_km + y_km * y_km + z_km * z_km);
+   
+   double accelFactor = - utils::math::Constants::earthGM_km / std::sqrt(r_km * r_km * r_km);
+   double ax = accelFactor * x_km * 1000.0;
+   double ay = accelFactor * y_km * 1000.0;
+   double az = accelFactor * z_km * 1000.0;
+
+   return std::make_tuple(ax, ay, az);
+}
+
+
+}

src/sim/SphericalGravityModel.h

@@ -0,0 +1,22 @@
+#ifndef SIM_SPHERICALGRAVITYMODEL_H
+#define SIM_SPHERICALGRAVITYMODEL_H
+
+#include "GravityModel.h"
+
+#include <tuple>
+
+namespace sim
+{
+
+class SphericalGravityModel : public GravityModel
+{
+public:
+   SphericalGravityModel();
+   virtual ~SphericalGravityModel();
+
+   std::tuple<double, double, double> getAccel(double x, double y, double z) override;
+};
+
+} // namespace sim
+
+#endif // SIM_SPHERICALGRAVITYMODEL_H

src/sim/USStandardAtmosphere.cpp

@@ -8,6 +8,56 @@ using namespace utils::math;
 namespace sim
 {
 
+// Populate static data
+utils::BinMap initTemps()
+{
+   utils::BinMap map;
+   map.insert(std::make_pair(0.0, 288.15));
+   map.insert(std::make_pair(11000.0, 216.65));
+   map.insert(std::make_pair(20000.0, 216.65));
+   map.insert(std::make_pair(32000.0, 228.65));
+   map.insert(std::make_pair(47000.0, 270.65));
+   map.insert(std::make_pair(51000.0, 270.65));
+   map.insert(std::make_pair(71000.0, 214.65));
+
+   return map;
+
+}
+
+utils::BinMap initLapseRates()
+{
+   utils::BinMap map;
+   map.insert(std::make_pair(0.0, -0.0019812));
+   map.insert(std::make_pair(11000.0, 0.0));
+   map.insert(std::make_pair(20000.0, 0.001));
+   map.insert(std::make_pair(32000.0, 0.0028));
+   map.insert(std::make_pair(47000.0, 0.0));
+   map.insert(std::make_pair(51000.0, -0.0028));
+   map.insert(std::make_pair(71000.0, -0.002));
+
+   return map;
+}
+
+utils::BinMap initDensities()
+{
+   utils::BinMap map;
+   map.insert(std::make_pair(0.0, 1.225));
+   map.insert(std::make_pair(11000.0, 0.36391));
+   map.insert(std::make_pair(20000.0, 0.08803));
+   map.insert(std::make_pair(32000.0, 0.01322));
+   map.insert(std::make_pair(47000.0, 0.00143));
+   map.insert(std::make_pair(51000.0, 0.00086));
+   map.insert(std::make_pair(71000.0, 0.000064));
+
+   return map;
+}
+
+utils::BinMap USStandardAtmosphere::temperatureLapseRate(initLapseRates());
+utils::BinMap USStandardAtmosphere::standardTemperature(initTemps());
+utils::BinMap USStandardAtmosphere::standardDensity(initDensities());
+
+
+
 USStandardAtmosphere::USStandardAtmosphere()
 {
 
@@ -20,7 +70,22 @@ USStandardAtmosphere::~USStandardAtmosphere()
 
 double USStandardAtmosphere::getDensity(double altitude)
 {
-   return Constants::standardDensity * std::exp((-Constants::g0 * Constants::airMolarMass * altitude) / (Constants::Rstar * Constants::standardTemperature));
+   if(temperatureLapseRate[altitude] == 0.0)
+   {
+      return standardDensity[altitude] * std::exp((-Constants::g0 * Constants::airMolarMass * altitude) / (Constants::Rstar * standardTemperature[altitude]));
+
+   }
+   else
+   {
+      double base = standardTemperature[altitude] /
+         (standardTemperature[altitude] + temperatureLapseRate[altitude] * (altitude - standardDensity.getBinBase(altitude)));
+      
+      double exponent = 1 + (Constants::g0 * Constants::airMolarMass) /
+         (Constants::Rstar * temperatureLapseRate[altitude]);
+
+      return standardDensity[altitude] * std::pow(base, exponent);
+
+   }
 }
 
 double USStandardAtmosphere::getTemperature(double altitude)

src/sim/USStandardAtmosphere.h

@@ -28,6 +28,9 @@ public:
    double getTemperature(double altitude) override;
 
 private:
+   static utils::BinMap temperatureLapseRate;
+   static utils::BinMap standardTemperature;
+   static utils::BinMap standardDensity;
 
 
 };

src/utils/BinMap.cpp

@@ -15,6 +15,13 @@ namespace utils
 {
 
 BinMap::BinMap()
+   : bins()
+{
+
+}
+
+BinMap::BinMap(BinMap&& o)
+   : bins(std::move(o.bins))
 {
 
 }
@@ -24,7 +31,9 @@ BinMap::~BinMap()
 
 }
 
-void BinMap::insert(std::pair<double, double>& toInsert)
+// TODO: Very low priority, but if anyone wants to make this more efficient it could be
+// interesting
+void BinMap::insert(const std::pair<double, double>& toInsert)
 {
    bins.push_back(toInsert);
    std::sort(bins.begin(), bins.end(),
@@ -59,4 +68,31 @@ double BinMap::operator[](double key)
    return retVal;
 }
 
+double BinMap::getBinBase(double key)
+{
+   auto iter = bins.begin();
+   // If the key is less than the lowest bin value, then it is out of range
+   // This should be an error. It's also possible to interpret this as simply
+   // the lowest bin, but I think that invites a logic error so we'll throw
+   // instead
+   if(key < iter->first)
+   {
+      throw std::out_of_range(
+         fmt::format("{} less than lower bound {} of BinMap", key, iter->first));
+   }
+   // Increment it and start searching If we reach the end without finding an existing key
+   // greater than our search term, then we've just hit the last bin and return that
+   iter++;
+   double retVal = bins.back().first;
+   while(iter !=  bins.end())
+   {
+      if(key < iter->first)
+      {
+         retVal = std::prev(iter)->first;
+         break;
+      }
+      iter++;
+   }
+   return retVal;
+}
 } // namespace utils

src/utils/BinMap.h

@@ -6,14 +6,27 @@
 
 namespace utils
 {
+
+/**
+ * @brief This is a utility class that operates as a map. Instead of a regular map
+ *        this one maps a range of key values to a single value. Like a bin, where
+ *        each key represents the bottom of the bin, and the next key represents the
+ *        bottom of the next bin, etc. When dereferencing the BinMap, it checks for
+ *        where the passed in key falls and returns the value in that bin.
+ * 
+ * @todo Make this class behave more like a proper STL container. Templetize it for one
+ * 
+ */
 class BinMap
 {
 public:
    BinMap();
+   BinMap(BinMap&& o);
    ~BinMap();
 
-   void insert(std::pair<double, double>& toInsert);
+   void insert(const std::pair<double, double>& toInsert);
    double operator[](double key);
+   double getBinBase(double key);
 
 private:
    std::vector<std::pair<double, double>> bins;

src/utils/math/Constants.h

@@ -11,6 +11,11 @@ namespace Constants
    constexpr const double airMolarMass = 0.0289644;
    constexpr const double standardTemperature = 288.15;
    constexpr const double standardDensity = 1.2250;
+   constexpr const double meanEarthRadiusWGS84 = 6371008.8;
+
+   constexpr const long double earthGM = 398600441800000.0;
+   constexpr const double earthGM_km = 398600.4418;
+
 };
 
 } // namespace utils::math