Added distance.cpp for the SphericalDistanceDeterminationAlgorithm

src/distance/distance.cpp

@@ -0,0 +1,71 @@
+#include <math.h>
+#include <utility>
+#include <iomanip>
+
+#include "spatial/attitude-utils.hpp"
+#include "spatial/camera.hpp"
+#include "style/style.hpp"
+
+#include "distance/distance.hpp"
+
+namespace found {
+
+PositionVector SphericalDistanceDeterminationAlgorithm::Run(const Points &p) {
+    // We do not normalize here, because the assumption for getting the distance
+    // is dependent on the fact that vec.x is 1. It is also not cost effective
+    // as the tradeoff is to nromalize center instead (3 norms v. 1 norm)
+    Vec3 spats[3] = {cam_.CameraToSpatial(p[0]),
+                      cam_.CameraToSpatial(p[1]),
+                      cam_.CameraToSpatial(p[2])};
+
+    // Obtain the center point of the projected circle
+    Vec3 center = std::move(getCenter(spats));
+
+    // Obtain the radius of the projected circle
+    decimal r = getRadius(spats, center);
+
+    // Obtain the distance from earth
+    decimal h = getDistance(r);
+
+    // You have to normalize the center vector here
+    return center.Normalize() * h;
+}
+
+Vec3 SphericalDistanceDeterminationAlgorithm::getCenter(Vec3 spats[3]) {
+    Vec3 diff1 = std::move(spats[1] - spats[0]);
+    Vec3 diff2 = std::move(spats[2] - spats[1]);
+
+    Vec3 circleN = std::move(diff1.CrossProduct(diff2));
+    Vec3 circlePt = spats[0];
+
+    Vec3 mid1 = std::move(midpoint(spats[0], spats[1]));
+    Vec3 mid2 = std::move(midpoint(spats[1], spats[2]));
+
+    Vec3 mid1N = std::move(diff1);
+    Vec3 mid2N = std::move(diff2);
+
+    Mat3 matrix;
+    matrix = {circleN.x, circleN.y, circleN.z, mid1N.x, mid1N.y,
+              mid1N.z, mid2N.x, mid2N.y, mid2N.z};
+
+    decimal alpha = circleN*circlePt;
+    decimal beta = mid1N*mid1;
+    decimal gamma = mid2N*mid2;
+
+    Vec3 y = {alpha, beta, gamma};
+
+    Vec3 center = std::move(matrix.Inverse() * y);
+
+    return center;
+}
+
+decimal SphericalDistanceDeterminationAlgorithm::getRadius(Vec3* spats,
+Vec3 center) {
+    return Distance(spats[0], center);
+}
+
+decimal SphericalDistanceDeterminationAlgorithm::getDistance(decimal r) {
+    return radius_*sqrt(r * r + 1)/r;
+}
+
+}  // namespace found

src/distance/distance.hpp

@@ -3,19 +3,21 @@
 
 #include "style/style.hpp"
 #include "pipeline/pipeline.hpp"
+#include "spatial/attitude-utils.hpp"
+#include "spatial/camera.hpp"
 
 namespace found {
 
 /**
  * The DistanceDeterminationAlgorithm class houses the Distance Determination Algorithm. This 
  * algorithm calculates the distance from Earth based on the pixels of Earth's Edge found in the image.
 */
-class DistanceDeterminationAlgorithm : public Stage<Points, distFromEarth> {
+class DistanceDeterminationAlgorithm : public Stage<Points, PositionVector> {
  public:
     // Constructs this
     DistanceDeterminationAlgorithm() = default;
     // Destroys this
-    virtual ~DistanceDeterminationAlgorithm();
+    virtual ~DistanceDeterminationAlgorithm() = default;
 };
 
 /**
@@ -26,20 +28,61 @@ class DistanceDeterminationAlgorithm : public Stage<Points, distFromEarth> {
 */
 class SphericalDistanceDeterminationAlgorithm : public DistanceDeterminationAlgorithm {
  public:
-   /**
-    * Initializes this SphericalDistanceDeterminationAlgorithm
-    * 
-    * @param radius The radius of Earth to use
-    */
-    explicit SphericalDistanceDeterminationAlgorithm(float radius);
-    ~SphericalDistanceDeterminationAlgorithm();
+    /**
+     * Creates a SphericalDeterminationAlgorithm, which deduces
+     * the Position vector of a sattelite from Earth by modeling
+     * Earth as a sphere
+     * 
+     * @param radius The radius of Earth
+     * @param cam The camera used to capture the picture of Earth
+     */
+    SphericalDistanceDeterminationAlgorithm(float radius, Camera &cam) : cam_(cam), radius_(radius) {}
+    ~SphericalDistanceDeterminationAlgorithm() {}
 
     /**
      * Place documentation here. Press enter to automatically make a new line
      * */
-    distFromEarth Run(const Points &p) override;
+    PositionVector Run(const Points &p) override;
+
  private:
     // Fields specific to this algorithm, and helper methods
+    /**
+     *Returns the center of earth as a 3d Vector
+     *
+     * @param spats The normalized spatial coordinates used to find the center 
+     * 
+     * @return The center of earth as a 3d Vector
+    */
+    Vec3 getCenter(Vec3* spats);
+
+    /**
+     * Returns the radius of the calculated "earth" (normalized)
+     * 
+     * @param spats The normalized spatial coordinates
+     * @param center The center of the earth as a 3d Vector
+     * 
+     * @return The radius of earth normalized
+    */
+    decimal getRadius(Vec3* spats, Vec3 center);
+
+    /**
+     * Returns the scaled distance from earth
+     * 
+     * @param r The normalized radius
+     * 
+     * @return The distance from earth as a Scalar
+    */
+    decimal getDistance(decimal r);
+
+    /**
+     * cam_ field instance describes the camera settings used for the photo taken
+    */
+    Camera cam_;
+
+    /**
+     * radius_ field instance describes the defined radius of earth. Should be 6378.0 (km)
+    */
+    float radius_;
 };
 
 /**
@@ -55,17 +98,17 @@ class EllipticDistanceDeterminationAlgorithm : public DistanceDeterminationAlgor
      * 
      * @param radius The distance from Earth to use
      */
-    explicit EllipticDistanceDeterminationAlgorithm(distFromEarth radius);
+    explicit EllipticDistanceDeterminationAlgorithm(PositionVector radius);
     ~EllipticDistanceDeterminationAlgorithm();
 
     /**
     * Place documentation here. Press enter to automatically make a new line
     * */
-    distFromEarth Run(const Points &p) override;
+    PositionVector Run(const Points &p) override;
  private:
     // Fields specific to this algorithm, and helper methods
 };
 
 }  // namespace found
 
-#endif
+#endif  // DISTANCE_H