Added distance.cpp for the SphericalDistanceDeterminationAlgorithm

src/attitude-utils.cpp

@@ -256,7 +256,6 @@ decimal DegToRad(decimal deg) {
  * @pre rad is in radians
  * 
  * @warning rad must be in radians
- * 
 */
 decimal RadToArcSec(decimal rad) {
     return RadToDeg(rad) * 3600.0;
@@ -270,7 +269,6 @@ decimal RadToArcSec(decimal rad) {
  * 
  * @return A possible angle value, in radians, corresponding
  * to the arcsecant value arcSec
- * 
 */
 decimal ArcSecToRad(decimal arcSec) {
     return DegToRad(arcSec / 3600.0);
@@ -377,7 +375,6 @@ Vec3 Vec3::operator-(const Vec3 &other) const {
  * 
  * @return A vector that is the cross product between
  * this and other
- * 
 */
 Vec3 Vec3::CrossProduct(const Vec3 &other) const {
     return {
@@ -394,7 +391,6 @@ Vec3 Vec3::CrossProduct(const Vec3 &other) const {
  * 
  * @return A matrix that is the outer product between this
  * and other
- * 
 */
 Mat3 Vec3::OuterProduct(const Vec3 &other) const {
     return {
@@ -425,14 +421,51 @@ Vec3 Vec3::operator*(const Mat3 &other) const {
 ///// VECTOR UTILITY FUNCTIONS ////
 ///////////////////////////////////
 
+/**
+ * Finds the midpoint between two different vectors
+ *
+ * @param vec1 The first vector
+ * @param vec2 The second vector
+ *
+ * @return The midpoint vector
+*/
+Vec2 midpoint(const Vec2 &vec1, const Vec2 &vec2){
+    return {(vec1.x + vec2.x)/2, (vec1.y + vec2.y)/2};
+}
+
+/**
+ * Finds the midpoint between two different vectors
+ *
+ * @param vec1 The first vector
+ * @param vec2 The second vector
+ *
+ * @return The midpoint vector
+*/
+Vec3 midpoint(const Vec3 &vec1, const Vec3 &vec2){
+    return {(vec1.x + vec2.x)/2, (vec1.y + vec2.y)/2, (vec1.z + vec2.z)/2};
+}
+
+/**
+ * Finds the midpoint between three different vectors
+ *
+ * @param vec1 The first vector
+ * @param vec2 The second vector
+ * @param vec3 The third vector
+ *
+ * @return The midpoint vector
+*/
+Vec3 midpoint(const Vec3 &vec1, const Vec3 &vec2, const Vec3 &vec3){
+    return {(vec1.x + vec2.x + vec3.x)/3, (vec1.y + vec2.y + vec3.y)/3, (vec1.z + vec2.z + vec3.z)/3};
+}
+
+
 /**
  * Determines the angle between two different vectors
  * 
  * @param vec1 The first vector
  * @param vec2 The second vector
  * 
  * @return The angle, in radians, between vec1 and vec2
- * 
 */
 decimal Angle(const Vec3 &vec1, const Vec3 &vec2) {
     return AngleUnit(vec1.Normalize(), vec2.Normalize());
@@ -457,16 +490,27 @@ decimal AngleUnit(const Vec3 &vec1, const Vec3 &vec2) {
 }
 
 /**
- * Determines the distance between two vectors
+ * Determines the Distance between two vectors
  * 
  * @param v1 The first vector
  * @param v2 The second vector
  * 
  * @return The distance between v1 and v2
- * 
 */
 decimal Distance(const Vec2 &v1, const Vec2 &v2) {
-    return sqrt(pow(v1.x-v2.x, 2) + pow(v1.y-v2.y, 2));
+    return (v1-v2).Magnitude();
+}
+
+/**
+ * Determines the Distance between two vectors
+ * 
+ * @param v1 The first vector
+ * @param v2 The second vector
+ * 
+ * @return The distance between v1 and v2
+*/
+decimal Distance(const Vec3 &v1, const Vec3 &v2) {
+    return (v1-v2).Magnitude();
 }
 
 ///////////////////////////////////
@@ -480,7 +524,6 @@ decimal Distance(const Vec2 &v1, const Vec2 &v2) {
  * @param j The column of the entry
  * 
  * @return The value of the entry in this at (i, j)
- * 
 */
 decimal Mat3::At(int i, int j) const {
     return x[3*i+j];
@@ -550,7 +593,6 @@ Mat3 Mat3::operator*(const decimal &s) const {
  * Obtains the transpose of this Matrix
  * 
  * @return The transpose Matrix of this
- * 
 */
 Mat3 Mat3::Transpose() const {
     return {
@@ -564,7 +606,6 @@ Mat3 Mat3::Transpose() const {
  * Obtains the trace of this Matrix
  * 
  * @return The trace of this
- * 
 */
 decimal Mat3::Trace() const {
     return At(0,0) + At(1,1) + At(2,2);
@@ -574,7 +615,6 @@ decimal Mat3::Trace() const {
  * Obtains the determinant of this Matrix
  * 
  * @return The determinant of this
- * 
 */
 decimal Mat3::Det() const {
     return (At(0,0) * (At(1,1)*At(2,2) - At(2,1)*At(1,2))) - (At(0,1) * (At(1,0)*At(2,2) - At(2,0)*At(1,2))) + (At(0,2) * (At(1,0)*At(2,1) - At(2,0)*At(1,1)));
@@ -583,8 +623,7 @@ decimal Mat3::Det() const {
 /**
  * Obtains the inverse of this Matrix
  * 
- * @return The inverse Matrix of this
- * 
+ * @return The inverse Matrix of this 
 */
 Mat3 Mat3::Inverse() const {
     // https://ardoris.wordpress.com/2008/07/18/general-formula-for-the-inverse-of-a-3x3-matrix/
@@ -612,8 +651,7 @@ Mat3 Mat3::Inverse() const {
 /**
  * Constructs an Attitude object from Quaternion information
  * 
- * @param quat The quaternion to base the attitude off of 
- * 
+ * @param quat The quaternion to base the attitude off of  
 */
 Attitude::Attitude(const Quaternion &quat)
     : quaternion(quat), type(QuaternionType) {}
@@ -623,7 +661,6 @@ Attitude::Attitude(const Quaternion &quat)
  * matrix holding the direction cosines for an attitude)
  * 
  * @param matrix The matrix holding the direction cosines
- * 
 */
 Attitude::Attitude(const Mat3 &matrix)
     : dcm(matrix), type(DCMType) {}
@@ -657,7 +694,6 @@ Mat3 QuaternionToDCM(const Quaternion &quat) {
  * @param dcm The matrix holding the direction cosines
  * 
  * @return A Quaternion that expresses the rotation defined in dcm
- * 
 */
 Quaternion DCMToQuaternion(const Mat3 &dcm) {
     // Make a quaternion that rotates the reference frame X-axis into the dcm's X-axis, just like
@@ -692,7 +728,6 @@ Quaternion DCMToQuaternion(const Mat3 &dcm) {
  * 
  * @return A Quaternion that holds the attitude information
  * of this
- * 
 */
 Quaternion Attitude::GetQuaternion() const {
     switch (type) {
@@ -710,7 +745,6 @@ Quaternion Attitude::GetQuaternion() const {
  * 
  * @return A matrix containing the direction cosines
  * indicated by this
- * 
 */
 Mat3 Attitude::GetDCM() const {
     switch (type) {
@@ -730,7 +764,6 @@ Mat3 Attitude::GetDCM() const {
  * @param vec The vector to rotate
  * 
  * @return A new vector that is rotated from vec based on this
- * 
 */
 Vec3 Attitude::Rotate(const Vec3 &vec) const {
     switch (type) {
@@ -748,7 +781,6 @@ Vec3 Attitude::Rotate(const Vec3 &vec) const {
  * 
  * @return An EulerAngles object that holds the Euler
  * Angles of this
- * 
 */
 EulerAngles Attitude::ToSpherical() const {
     switch (type) {
@@ -769,7 +801,6 @@ EulerAngles Attitude::ToSpherical() const {
  * Computes the size, in bytes, that a Vec3 object will take up
  * 
  * @return The number of bytes that a Vec3 occupies
- * 
 */
 long SerializeLengthVec3() {
     return sizeof(decimal)*3;
@@ -787,7 +818,6 @@ long SerializeLengthVec3() {
  * @note A buffer is a very long character array that holds information
  * that the user defines. Serialization of data means inputting certain
  * data into a buffer.
- * 
 */
 void SerializeVec3(const Vec3 &vec, unsigned char *buffer) {
     decimal *fBuffer = (decimal *)buffer;
@@ -809,7 +839,6 @@ void SerializeVec3(const Vec3 &vec, unsigned char *buffer) {
  * 
  * @warning Returns nonsense if buffer does not point to a valid location
  * that stores a Vec3
- * 
 */
 Vec3 DeserializeVec3(const unsigned char *buffer) {
     Vec3 result;

src/attitude-utils.hpp

@@ -224,6 +224,11 @@ class Attitude {
     AttitudeType type;
 };
 
+// Vector operations
+Vec2 midpoint(const Vec2 &, const Vec2 &);
+Vec3 midpoint(const Vec3 &, const Vec3 &);
+Vec3 midpoint(const Vec3 &, const Vec3 &, const Vec3);
+
 // DCM-Quaternion-Spherical Conversions
 
 Mat3 QuaternionToDCM(const Quaternion &);

src/distance.cpp

@@ -0,0 +1,64 @@
+#include <math.h>
+
+#include "attitude-utils.hpp"
+#include "style.hpp"
+#include "camera.hpp"
+
+#include "distance.hpp"
+
+#define RADIUS_OF_EARTH 6378.0
+
+namespace found {  
+
+distFromEarth SphericalDistanceDeterminationAlgorithm::Run(char *image, Points &p, int imageWidth, int imageHeight)
+{
+    return solve(p, radius_);
+}
+
+Vec3 SphericalDistanceDeterminationAlgorithm::getCenter(Vec3* spats){
+    Vec3 diff1 = spats[1] - spats[0];
+    Vec3 diff2 = spats[2] -spats[1];
+
+    Vec3 circleN = diff1.CrossProduct(diff2);
+    Vec3 circlePt = spats[0];
+
+    Vec3 mid1 = midpoint(spats[0], spats[1]);
+    Vec3 mid2 = midpoint(spats[1], spats[2]);
+
+    Vec3 mid1N = diff1;
+    Vec3 mid2N = diff2;
+
+    Mat3 matrix;
+    matrix = {circleN.x, circleN.y, circleN.z, mid1N.x, mid1N.y, mid1N.z, mid2N.x, mid2N.y, mid2N.z};
+
+    int alpha = circleN*circlePt;
+    int beta = mid1N*mid1;
+    int gamma = mid2N*mid2;
+
+    Vec3 y = {alpha, beta, gamma};
+
+    Vec3 center = matrix.Inverse() * y;
+
+    return center;
+}
+
+decimal SphericalDistanceDeterminationAlgorithm::getRadius(Vec3* spats, Vec3 center){
+    return Distance(spats[0], center);
+}
+
+decimal SphericalDistanceDeterminationAlgorithm::getDistance(decimal r)
+{
+    return RADIUS_OF_EARTH/r;
+}
+
+distFromEarth SphericalDistanceDeterminationAlgorithm::solve(Points& pts, int R){
+    Vec3 uSpats[3] = {cam_.CameraToSpatial(pts[0]).Normalize(), cam_.CameraToSpatial(pts[1]).Normalize(), cam_.CameraToSpatial(pts[2]).Normalize()};
+
+    Vec3 center = getCenter(uSpats);
+    decimal r = getRadius(uSpats, center);
+
+    decimal h = getDistance(r);
+
+    return center * h;
+}
+};

src/distance.hpp

@@ -2,6 +2,8 @@
 #define DISTANCE_H
 
 #include "style.hpp"
+#include "attitude-utils.hpp"
+#include "camera.hpp"
 
 namespace found {
 
@@ -25,7 +27,7 @@ class DistanceDeterminationAlgorithm {
      * 
      * @return The distance of the satellite from Earth
     */
-    virtual distFromEarth Run(char* image, Points &p /*More go here*/) = 0;
+    virtual distFromEarth Run(char* image, Points &p, int imageWidth, int imageHeight /*More go here*/) = 0;
 };
 
 /**
@@ -36,15 +38,56 @@ class DistanceDeterminationAlgorithm {
 */
 class SphericalDistanceDeterminationAlgorithm : public DistanceDeterminationAlgorithm {
 public:
-    SphericalDistanceDeterminationAlgorithm(float radius);
+    SphericalDistanceDeterminationAlgorithm(float radius, Camera &cam) : radius_(radius), cam_(cam) {};
     ~SphericalDistanceDeterminationAlgorithm();
 
     /**
      * Place documentation here. Press enter to automatically make a new line
      * */
-    distFromEarth Run(char* image, Points &p/*More go here*/) override;
+    distFromEarth Run(char* image, Points &p, int imageWidth, int imageHeight/*More go here*/) 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);
+
+    /**
+     * Solves the whole thing, calculating the final distance from the earth as a 3d Vector
+     * 
+     * @param pts The points on the image (Not used)
+     * @param R The given radius (Currently not used, Radius is set to 6378.0)
+     * 
+     * @return The distance from earth as a 3d Vector
+    */
+    distFromEarth solve(Points& pts, int R);
+
+    Camera cam_;
+    float radius_;
+
 };
 
 /**