From 1328d9ffd60c31f1ca505eb874f0606e2bb59b11 Mon Sep 17 00:00:00 2001 From: Muki Kiboigo Date: Tue, 28 Nov 2023 13:04:43 -0800 Subject: [PATCH] fix: switch all math functions to decimal wrapped ones --- src/attitude-utils.cpp | 49 +++++++++++++++++++++--------------------- src/attitude-utils.hpp | 2 +- src/decimal.hpp | 6 ++++++ src/star-id.cpp | 10 ++++----- 4 files changed, 37 insertions(+), 30 deletions(-) diff --git a/src/attitude-utils.cpp b/src/attitude-utils.cpp index 24dca02a..ab30c8f4 100644 --- a/src/attitude-utils.cpp +++ b/src/attitude-utils.cpp @@ -1,5 +1,6 @@ #include "attitude-utils.hpp" +#include #include #include #include @@ -43,11 +44,11 @@ Quaternion::Quaternion(const Vec3 &input) { /// Create a quaternion which represents a rotation of theta around the axis input Quaternion::Quaternion(const Vec3 &input, decimal theta) { - real = cos(theta/2); + real = DECIMAL_COS(theta/2); // the compiler will optimize it. Right? - i = input.x * sin(theta/2); - j = input.y * sin(theta/2); - k = input.z * sin(theta/2); + i = input.x * DECIMAL_SIN(theta/2); + j = input.y * DECIMAL_SIN(theta/2); + k = input.z * DECIMAL_SIN(theta/2); } /// Rotate a 3d vector according to the rotation represented by the quaternion. @@ -62,7 +63,7 @@ decimal Quaternion::Angle() const { return 0; // 180*2=360=0 } // TODO: we shouldn't need this nonsense, right? how come acos sometimes gives nan? (same as in AngleUnit) - return (real >= 1 ? 0 : acos(real))*2; + return (real >= 1 ? 0 : DECIMAL_ACOS(real))*2; } decimal Quaternion::SmallestAngle() const { @@ -73,8 +74,8 @@ decimal Quaternion::SmallestAngle() const { } void Quaternion::SetAngle(decimal newAngle) { - real = cos(newAngle/2); - SetVector(Vector().Normalize() * sin(newAngle/2)); + real = DECIMAL_COS(newAngle/2); + SetVector(Vector().Normalize() * DECIMAL_SIN(newAngle/2)); } EulerAngles Quaternion::ToSpherical() const { @@ -85,11 +86,11 @@ EulerAngles Quaternion::ToSpherical() const { // and 2, we store the conjugate of the quaternion (double check why?), which means we need to // invert the final de and roll terms, as well as negate all the terms involving a mix between // the real and imaginary parts. - decimal ra = atan2(2*(-real*k+i*j), 1-2*(j*j+k*k)); + decimal ra = DECIMAL_ATAN2(2*(-real*k+i*j), 1-2*(j*j+k*k)); if (ra < 0) ra += 2*DECIMAL_M_PI; - decimal de = -asin(2*(-real*j-i*k)); // allow de to be positive or negaive, as is convention - decimal roll = -atan2(2*(-real*i+j*k), 1-2*(i*i+j*j)); + decimal de = -DECIMAL_ASIN(2*(-real*j-i*k)); // allow de to be positive or negaive, as is convention + decimal roll = -DECIMAL_ATAN2(2*(-real*i+j*k), 1-2*(i*i+j*j)); if (roll < 0) roll += 2*DECIMAL_M_PI; @@ -134,18 +135,18 @@ Quaternion Quaternion::Canonicalize() const { /// Convert from right ascension & declination to a 3d point on the unit sphere. Vec3 SphericalToSpatial(decimal ra, decimal de) { return { - cos(ra)*cos(de), - sin(ra)*cos(de), - sin(de), + DECIMAL_COS(ra)*DECIMAL_COS(de), + DECIMAL_SIN(ra)*DECIMAL_COS(de), + DECIMAL_SIN(de), }; } /// Convert from a 3d point on the unit sphere to right ascension & declination. void SpatialToSpherical(const Vec3 &vec, decimal *ra, decimal *de) { - *ra = atan2(vec.y, vec.x); + *ra = DECIMAL_ATAN2(vec.y, vec.x); if (*ra < 0) *ra += DECIMAL_M_PI*2; - *de = asin(vec.z); + *de = DECIMAL_ASIN(vec.z); } decimal RadToDeg(decimal rad) { @@ -164,28 +165,28 @@ decimal ArcSecToRad(decimal arcSec) { return DegToRad(arcSec / DECIMAL(3600.0)); } -decimal FloatModulo(decimal x, decimal mod) { +decimal DecimalModulo(decimal x, decimal mod) { // first but not last chatgpt generated code in lost: - decimal result = x - mod * floor(x / mod); + decimal result = x - mod * DECIMAL_FLOOR(x / mod); return result >= 0 ? result : result + mod; } /// The square of the magnitude decimal Vec3::MagnitudeSq() const { - return fma(x,x,fma(y,y, z*z)); + return DECIMAL_FMA(x,x,DECIMAL_FMA(y,y, z*z)); } /// The square of the magnitude decimal Vec2::MagnitudeSq() const { - return fma(x,x, y*y); + return DECIMAL_FMA(x,x, y*y); } decimal Vec3::Magnitude() const { - return hypot(hypot(x, y), z); // not sure if this is faster than a simple sqrt, but it does have less error? + return DECIMAL_HYPOT(DECIMAL_HYPOT(x, y), z); // not sure if this is faster than a simple sqrt, but it does have less error? } decimal Vec2::Magnitude() const { - return hypot(x, y); + return DECIMAL_HYPOT(x, y); } /// Create a vector pointing in the same direction with magnitude 1 @@ -198,7 +199,7 @@ Vec3 Vec3::Normalize() const { /// Dot product decimal Vec3::operator*(const Vec3 &other) const { - return fma(x,other.x, fma(y,other.y, z*other.z)); + return DECIMAL_FMA(x,other.x, DECIMAL_FMA(y,other.y, z*other.z)); } /// Dot product @@ -368,7 +369,7 @@ Quaternion DCMToQuaternion(const Mat3 &dcm) { // the DCM itself does Vec3 oldXAxis = Vec3({1, 0, 0}); Vec3 newXAxis = dcm.Column(0); // this is where oldXAxis is mapped to - assert(abs(newXAxis.Magnitude()-1) < DECIMAL(0.001)); + assert(DECIMAL_ABS(newXAxis.Magnitude()-1) < DECIMAL(0.001)); Vec3 xAlignAxis = oldXAxis.CrossProduct(newXAxis).Normalize(); decimal xAlignAngle = AngleUnit(oldXAxis, newXAxis); Quaternion xAlign(xAlignAxis, xAlignAngle); @@ -478,7 +479,7 @@ decimal Angle(const Vec3 &vec1, const Vec3 &vec2) { decimal AngleUnit(const Vec3 &vec1, const Vec3 &vec2) { decimal dot = vec1*vec2; // TODO: we shouldn't need this nonsense, right? how come acos sometimes gives nan? - return dot >= 1 ? 0 : dot <= -1 ? DECIMAL_M_PI-DECIMAL(0.0000001) : acos(dot); + return dot >= 1 ? 0 : dot <= -1 ? DECIMAL_M_PI-DECIMAL(0.0000001) : DECIMAL_ACOS(dot); } } diff --git a/src/attitude-utils.hpp b/src/attitude-utils.hpp index fe285879..0935afc8 100644 --- a/src/attitude-utils.hpp +++ b/src/attitude-utils.hpp @@ -182,7 +182,7 @@ decimal RadToArcSec(decimal); decimal ArcSecToRad(decimal); /// Given a decimal, find it "modulo" another decimal, in the true mathematical sense (not remainder). /// Always returns something in [0,mod) Eg -0.8 mod 0.6 = 0.4 -decimal FloatModulo(decimal x, decimal mod); +decimal DecimalModulo(decimal x, decimal mod); // TODO: quaternion and euler angle conversion, conversion between ascension/declination to rec9tu diff --git a/src/decimal.hpp b/src/decimal.hpp index 218a51ef..16689ea1 100644 --- a/src/decimal.hpp +++ b/src/decimal.hpp @@ -43,6 +43,7 @@ #define DECIMAL_ROUND(x) (decimal) std::round(x) #define DECIMAL_CEIL(x) (decimal) std::ceil(x) #define DECIMAL_FLOOR(x) (decimal) std::floor(x) +#define DECIMAL_ABS(x) (decimal) std::abs(x) // Trig Methods wrapped with Decimal typecast #define DECIMAL_SIN(x) (decimal) std::sin(x) @@ -51,5 +52,10 @@ #define DECIMAL_ASIN(x) (decimal) std::asin(x) #define DECIMAL_ACOS(x) (decimal) std::acos(x) #define DECIMAL_ATAN(x) (decimal) std::atan(x) +#define DECIMAL_ATAN2(x,y) (decimal) std::atan2(x,y) + +// Float methods wrapped with Decimal typecast +#define DECIMAL_FMA(x,y,z) (decimal) std::fma(x,y,z) +#define DECIMAL_HYPOT(x,y) (decimal) std::hypot(x,y) #endif // decimal.hpp diff --git a/src/star-id.cpp b/src/star-id.cpp index 1a3e4c97..7de2c303 100644 --- a/src/star-id.cpp +++ b/src/star-id.cpp @@ -122,7 +122,7 @@ StarIdentifiers GeometricVotingStarIdAlgorithm::Go( //if sDist is in the range of (distance between stars in the image +- R) //add a vote for the match - if (abs(sDist - cDist) < tolerance) { + if (DECIMAL_ABS(sDist - cDist) < tolerance) { verificationVotes[i]++; verificationVotes[j]++; } @@ -266,7 +266,7 @@ std::unordered_multimap PairDistanceQueryToMap(const int16_t * } decimal IRUnidentifiedCentroid::VerticalAnglesToAngleFrom90(decimal v1, decimal v2) { - return abs(FloatModulo(v1-v2, DECIMAL_M_PI) - DECIMAL_M_PI_2); + return DECIMAL_ABS(DecimalModulo(v1-v2, DECIMAL_M_PI) - DECIMAL_M_PI_2); } /** @@ -276,7 +276,7 @@ decimal IRUnidentifiedCentroid::VerticalAnglesToAngleFrom90(decimal v1, decimal void IRUnidentifiedCentroid::AddIdentifiedStar(const StarIdentifier &starId, const Stars &stars) { const Star &otherStar = stars[starId.starIndex]; Vec2 positionDifference = otherStar.position - star->position; - decimal angleFromVertical = atan2(positionDifference.y, positionDifference.x); + decimal angleFromVertical = DECIMAL_ATAN2(positionDifference.y, positionDifference.x); for (const auto &otherPair : identifiedStarsInRange) { decimal curAngleFrom90 = VerticalAnglesToAngleFrom90(otherPair.first, angleFromVertical); @@ -302,8 +302,8 @@ std::vector::iterator> FindUnidentifiedCen Vec3 ourSpatial = camera.CameraToSpatial(star.position).Normalize(); - decimal minCos = cos(maxDistance); - decimal maxCos = cos(minDistance); + decimal minCos = DECIMAL_COS(maxDistance); + decimal maxCos = DECIMAL_COS(minDistance); std::vector::iterator> result; for (auto it = centroids->begin(); it != centroids->end(); ++it) {