Add quaternion constants in celmath, use sincos in rotation matrix fu…

…nctions
CelestiaProject · Oct 25, 2023 · db3e89c · db3e89c
1 parent 53e5508
commit db3e89c
Show file tree

Hide file tree

Showing 11 changed files with 70 additions and 39 deletions.
diff --git a/src/celengine/axisarrow.cpp b/src/celengine/axisarrow.cpp
@@ -11,6 +11,7 @@
 #include <algorithm>
 #include <vector>
 #include <celcompat/numbers.h>
+#include <celmath/geomutil.h>
 #include <celmath/mathlib.h>
 #include <celmath/vecgl.h>
 #include <celrender/linerenderer.h>
@@ -308,19 +309,19 @@ AxesReferenceMark::render(Renderer* renderer,
     Eigen::Matrix4f labelTransformMatrix = labelTransform.matrix();
 
     // x-axis
-    Eigen::Matrix4f xModelView = modelView * celmath::rotate(Eigen::AngleAxisf(90.0_deg, Eigen::Vector3f::UnitY()));
+    Eigen::Matrix4f xModelView = modelView * celmath::rotate(celmath::YRot90<float>);
     glVertexAttrib4f(CelestiaGLProgram::ColorAttributeIndex, 1.0f, 0.0f, 0.0f, opacity);
     prog->setMVPMatrices(projection, xModelView);
     GetArrowVAO().draw();
 
     // y-axis
-    Eigen::Matrix4f yModelView = modelView * celmath::rotate(Eigen::AngleAxisf(180.0_deg, Eigen::Vector3f::UnitY()));
+    Eigen::Matrix4f yModelView = modelView * celmath::rotate(celmath::YRot180<float>);
     glVertexAttrib4f(CelestiaGLProgram::ColorAttributeIndex, 0.0f, 1.0f, 0.0f, opacity);
     prog->setMVPMatrices(projection, yModelView);
     GetArrowVAO().draw();
 
     // z-axis
-    Eigen::Matrix4f zModelView = modelView * celmath::rotate(Eigen::AngleAxisf(-90.0_deg, Eigen::Vector3f::UnitX()));
+    Eigen::Matrix4f zModelView = modelView * celmath::rotate(celmath::XRot90Conjugate<float>);
     glVertexAttrib4f(CelestiaGLProgram::ColorAttributeIndex, 0.0f, 0.0f, 1.0f, opacity);
     prog->setMVPMatrices(projection, zModelView);
     GetArrowVAO().draw();
@@ -455,7 +456,7 @@ BodyAxisArrows::BodyAxisArrows(const Body& _body) :
 Eigen::Quaterniond
 BodyAxisArrows::getOrientation(double tdb) const
 {
-    return (Eigen::Quaterniond(Eigen::AngleAxis<double>(celestia::numbers::pi, Eigen::Vector3d::UnitY())) * body.getEclipticToBodyFixed(tdb)).conjugate();
+    return (celmath::YRot180<double> * body.getEclipticToBodyFixed(tdb)).conjugate();
 }
 
 

diff --git a/src/celengine/observer.cpp b/src/celengine/observer.cpp
@@ -538,7 +538,7 @@ void Observer::setLocationFilter(uint64_t _locationFilter)
 
 void Observer::reverseOrientation()
 {
-    setOrientation(getOrientation() * Quaterniond(AngleAxisd(celestia::numbers::pi, Vector3d::UnitY())));
+    setOrientation(getOrientation() * celmath::YRot180<double>);
     reverseFlag = !reverseFlag;
 }
 

diff --git a/src/celengine/planetgrid.cpp b/src/celengine/planetgrid.cpp
@@ -14,6 +14,7 @@
 #include <Eigen/Geometry>
 #include <fmt/format.h>
 #include <celcompat/numbers.h>
+#include <celmath/geomutil.h>
 #include <celmath/intersect.h>
 #include <celmath/vecgl.h>
 #include <celrender/linerenderer.h>
@@ -99,8 +100,7 @@ PlanetographicGrid::render(Renderer* renderer,
     InitializeGeometry(*renderer);
 
     // Compatibility
-    Eigen::Quaterniond q(Eigen::AngleAxis(celestia::numbers::pi, Eigen::Vector3d::UnitY()));
-    q *= body.getEclipticToBodyFixed(tdb);
+    Eigen::Quaterniond q = celmath::YRot180<double> * body.getEclipticToBodyFixed(tdb);
     Eigen::Quaternionf qf = q.cast<float>();
 
     // The grid can't be rendered exactly on the planet sphere, or

diff --git a/src/celengine/skygrid.cpp b/src/celengine/skygrid.cpp
@@ -335,10 +335,6 @@ SkyGrid::render(Renderer& renderer,
                 int windowWidth,
                 int windowHeight)
 {
-    // 90 degree rotation about the x-axis used to transform coordinates
-    // to Celestia's system.
-    Quaterniond xrot90 = XRotation(-celestia::numbers::pi / 2.0);
-
     auto vfov = static_cast<double>(renderer.getProjectionMode()->getFOV(observer.getZoom()));
     double viewAspectRatio = static_cast<double>(windowWidth) / static_cast<double>(windowHeight);
 
@@ -369,7 +365,13 @@ SkyGrid::render(Renderer& renderer,
     Vector3d c3( w,  h, -1.0);
 
     Quaterniond cameraOrientation = renderer.getCameraOrientation();
-    Matrix3d r = (cameraOrientation * xrot90 * m_orientation.conjugate() * xrot90.conjugate()).toRotationMatrix().transpose();
+
+    // 90 degree rotation about the x-axis used to transform coordinates
+    // to Celestia's system.
+    Matrix3d r = (cameraOrientation *
+                  celmath::XRot90Conjugate<double> *
+                  m_orientation.conjugate() *
+                  celmath::XRot90<double>).toRotationMatrix().transpose();
 
     // Transform the frustum corners by the camera and grid
     // rotations.
@@ -481,13 +483,15 @@ SkyGrid::render(Renderer& renderer,
     int endDec   = (int) std::floor(DEG_MIN_SEC_TOTAL  * (maxDec / celestia::numbers::pi) / (double) decIncrement) * decIncrement;
 
     // Get the orientation at single precision
-    Quaterniond q = xrot90 * m_orientation * xrot90.conjugate();
+    Quaterniond q = celmath::XRot90Conjugate<double> * m_orientation * celmath::XRot90<double>;
     Quaternionf orientationf = q.cast<float>();
 
     // Radius of sphere is arbitrary, with the constraint that it shouldn't
     // intersect the near or far plane of the view frustum.
     Matrix4f m = renderer.getModelViewMatrix() *
-                 celmath::rotate((xrot90 * m_orientation.conjugate() * xrot90.conjugate()).cast<float>()) *
+                 celmath::rotate((celmath::XRot90Conjugate<double> *
+                                  m_orientation.conjugate() *
+                                  celmath::XRot90<double>).cast<float>()) *
                  celmath::scale(1000.0f);
     Matrices matrices = {&renderer.getProjectionMatrix(), &m};
 

diff --git a/src/celephem/customorbit.cpp b/src/celephem/customorbit.cpp
@@ -1366,8 +1366,8 @@ class PhobosOrbit : public CachingOrbit
 
         // Orientation of the orbital plane with respect to the Laplacian plane
         Eigen::Matrix3d Rorbit = (celmath::YRotation(node) *
-                                 celmath::XRotation(celmath::degToRad(i)) *
-                                 celmath::YRotation(w)).toRotationMatrix();
+                                  celmath::XRotation(celmath::degToRad(i)) *
+                                  celmath::YRotation(w)).toRotationMatrix();
 
         // Rotate to the Earth's equatorial plane
         constexpr double N = celmath::degToRad(refplane_RA);

diff --git a/src/celephem/customrotation.cpp b/src/celephem/customrotation.cpp
@@ -90,7 +90,7 @@ class IAURotationModel : public CachingRotationModel
         double inclination = 90.0 - poleDec;
 
         if (flipped)
-            return celmath::XRotation(celestia::numbers::pi) *
+            return celmath::XRot180<double> *
                    celmath::XRotation(celmath::degToRad(-inclination)) *
                    celmath::YRotation(celmath::degToRad(-node));
         else
@@ -176,9 +176,7 @@ class EarthRotationModel : public CachingRotationModel
         Eigen::Quaterniond q = celmath::XRotation(obliquity) * celmath::ZRotation(-precession) * eclRotation.conjugate();
 
         // convert to Celestia's coordinate system
-        return celmath::XRotation(celestia::numbers::pi / 2.0) *
-               q *
-               celmath::XRotation(-celestia::numbers::pi / 2.0);
+        return celmath::XRot90<double> * q * celmath::XRot90Conjugate<double>;
     }
 
     double getPeriod() const override

diff --git a/src/celephem/samporient.cpp b/src/celephem/samporient.cpp
@@ -111,14 +111,12 @@ class SampledOrientation : public RotationModel
 void
 SampledOrientation::addSample(double t, const Eigen::Quaternionf& q)
 {
-    // 90 degree rotation about x-axis to convert orientation to Celestia's
-    // coordinate system.
-    static const Eigen::Quaternionf coordSysCorrection = celmath::XRotation((float) (celestia::numbers::pi / 2.0));
-
     // TODO: add a check for out of sequence samples
     OrientationSample& samp = samples.emplace_back();
     samp.t = t;
-    samp.q = q * coordSysCorrection;
+    // 90 degree rotation about x-axis to convert orientation to Celestia's
+    // coordinate system.
+    samp.q = q * celmath::XRot90<float>;
 }
 
 

diff --git a/src/celephem/spicerotation.cpp b/src/celephem/spicerotation.cpp
@@ -182,9 +182,10 @@ SpiceRotation::computeSpin(double jd) const
         Eigen::Quaterniond q = Eigen::Quaterniond(Eigen::Map<Eigen::Matrix3d>(matrixData)).conjugate();
 
         // Transform into Celestia's coordinate system
-        static const Eigen::Quaterniond Rx90 = celmath::XRotation(celestia::numbers::pi / 2.0);
-        static const Eigen::Quaterniond Ry180 = celmath::YRotation(celestia::numbers::pi);
-        return Ry180 * Rx90.conjugate() * q.conjugate() * Rx90;
+        return celmath::YRot180<double> *
+               celmath::XRot90Conjugate<double> *
+               q.conjugate() *
+               celmath::XRot90<double>;
     }
 }
 

diff --git a/src/celestia/gtk/dialog-eclipse.cpp b/src/celestia/gtk/dialog-eclipse.cpp
@@ -332,7 +332,7 @@ static gint eclipseGoto(GtkButton*, EclipseData* ed)
 
     double distance = target.radius() * 4.0;
     sim->gotoLocation(UniversalCoord::Zero().offsetKm(Vector3d::UnitX() * distance),
-                                  (YRotation(-celestia::numbers::pi / 2) * XRotation(-celestia::numbers::pi / 2)), 2.5);
+                      (celmath::YRot90Conjugate<double> * celmath::XRot90Conjugate<double>), 2.5);
 
     return TRUE;
 }

diff --git a/src/celestia/win32/wineclipses.cpp b/src/celestia/win32/wineclipses.cpp
@@ -390,7 +390,7 @@ BOOL APIENTRY EclipseFinderProc(HWND hDlg,
 
                 double distance = target.radius() * 4.0;
                 sim->gotoLocation(UniversalCoord::Zero().offsetKm(Vector3d::UnitX() * distance),
-                                  YRotation(-celestia::numbers::pi / 2) * XRotation(-celestia::numbers::pi / 2),
+                                  celmath::YRot90Conjugate<double> * celmath::XRot90Conjugate<double>,
                                   2.5);
             }
             break;

diff --git a/src/celmath/geomutil.h b/src/celmath/geomutil.h
@@ -15,35 +15,64 @@
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 
+#include <celcompat/numbers.h>
 #include "mathlib.h"
 
 namespace celmath
 {
 
+template<class T> const inline Eigen::Quaternion<T> XRot90{ celestia::numbers::sqrt2_v<T> * T{0.5},
+                                                            celestia::numbers::sqrt2_v<T> * T{0.5},
+                                                            T{0},
+                                                            T{0} };
+
+template<class T> const inline Eigen::Quaternion<T> XRot180{ 0, 1, 0, 0 };
+
+// Conjugate of 90 degree rotation = -90 degree rotation
+template<class T> const inline Eigen::Quaternion<T> XRot90Conjugate{ celestia::numbers::sqrt2_v<T> * T{0.5},
+                                                                     -(celestia::numbers::sqrt2_v<T> * T{0.5}),
+                                                                     T{0},
+                                                                     T{0} };
+
+template<class T> const inline Eigen::Quaternion<T> YRot90{ celestia::numbers::sqrt2_v<T> * T{0.5},
+                                                            T{0},
+                                                            celestia::numbers::sqrt2_v<T> * T{0.5},
+                                                            T{0} };
+
+template<class T> const inline Eigen::Quaternion<T> YRot180{ 0, 0, 1, 0 };
+
+template<class T> const inline Eigen::Quaternion<T> YRot90Conjugate{ celestia::numbers::sqrt2_v<T> * T{0.5},
+                                                                     T{0},
+                                                                     -(celestia::numbers::sqrt2_v<T> * T{0.5}),
+                                                                     T{0} };
+
 template<class T> Eigen::Quaternion<T>
 XRotation(T radians)
 {
-    using std::cos, std::sin;
-    T halfAngle = radians * static_cast<T>(0.5);
-    return Eigen::Quaternion<T>(cos(halfAngle), sin(halfAngle), 0, 0);
+    T s;
+    T c;
+    sincos(radians * static_cast<T>(0.5), s, c);
+    return Eigen::Quaternion<T>(c, s, 0, 0);
 }
 
 
 template<class T> Eigen::Quaternion<T>
 YRotation(T radians)
 {
-    using std::cos, std::sin;
-    T halfAngle = radians * static_cast<T>(0.5);
-    return Eigen::Quaternion<T>(cos(halfAngle), 0, sin(halfAngle), 0);
+    T s;
+    T c;
+    sincos(radians * static_cast<T>(0.5), s, c);
+    return Eigen::Quaternion<T>(c, 0, s, 0);
 }
 
 
 template<class T> Eigen::Quaternion<T>
 ZRotation(T radians)
 {
-    using std::cos, std::sin;
-    T halfAngle = radians * static_cast<T>(0.5);
-    return Eigen::Quaternion<T>(cos(halfAngle), 0, 0, sin(halfAngle));
+    T s;
+    T c;
+    sincos(radians * static_cast<T>(0.5), s, c);
+    return Eigen::Quaternion<T>(c, 0, 0, s);
 }