Merge pull request #348 from Exawind/tangent-twist-quat

Quaternion from tangent and twist

src/math/quaternion_operations.hpp

@@ -5,6 +5,7 @@
 #include <Kokkos_Core.hpp>
 
 #include "types.hpp"
+#include "vector_operations.hpp"
 
 namespace openturbine {
 
@@ -49,6 +50,63 @@ inline std::array<Array_3, 3> QuaternionToRotationMatrix(const Array_4& q) {
     }};
 }
 
+/**
+ * @brief Converts a 3x3 rotation matrix to a 4x1 quaternion and returns the result, see
+ * https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ for
+ * implementation details
+ */
+inline Array_4 RotationMatrixToQuaternion(const Array_3x3& m) {
+    auto m22_p_m33 = m[1][1] + m[2][2];
+    auto m22_m_m33 = m[1][1] - m[2][2];
+    Array_4 vals{
+        m[0][0] + m22_p_m33,
+        m[0][0] - m22_p_m33,
+        -m[0][0] + m22_m_m33,
+        -m[0][0] - m22_m_m33,
+    };
+
+    // Get maximum value and index of maximum value
+    auto* max_num = std::max_element(vals.begin(), vals.end());
+    auto max_idx = std::distance(vals.begin(), max_num);
+
+    auto tmp = sqrt(*max_num + 1.);
+    auto c = 0.5 / tmp;
+
+    if (max_idx == 0) {
+        return Array_4{
+            0.5 * tmp,
+            (m[2][1] - m[1][2]) * c,
+            (m[0][2] - m[2][0]) * c,
+            (m[1][0] - m[0][1]) * c,
+        };
+    }
+    if (max_idx == 1) {
+        return Array_4{
+            (m[2][1] - m[1][2]) * c,
+            0.5 * tmp,
+            (m[0][1] + m[1][0]) * c,
+            (m[0][2] + m[2][0]) * c,
+        };
+    }
+    if (max_idx == 2) {
+        return Array_4{
+            (m[0][2] - m[2][0]) * c,
+            (m[0][1] + m[1][0]) * c,
+            0.5 * tmp,
+            (m[1][2] + m[2][1]) * c,
+        };
+    }
+    if (max_idx == 3) {
+        return Array_4{
+            (m[1][0] - m[0][1]) * c,
+            (m[0][2] + m[2][0]) * c,
+            (m[1][2] + m[2][1]) * c,
+            0.5 * tmp,
+        };
+    }
+    return Array_4{1., 0., 0., 0};
+}
+
 /**
  * @brief Rotates provided vector by provided *unit* quaternion and returns the result
  */
@@ -245,4 +303,34 @@ Kokkos::Array<double, 4> NormalizeQuaternion(const Kokkos::Array<double, 4>& q)
     return normalized_quaternion;
 }
 
+/**
+ * @brief Returns a 4-D quaternion from provided tangent vector and twist (degrees) about tangent
+ */
+inline Array_4 TangentTwistToQuaternion(const Array_3& tangent, const double twist) {
+    const auto e1 = UnitVector(tangent);
+    Array_3 temp{0., 1., 0.};
+    if (std::abs(Dot3(e1, temp)) > 0.9) {
+        temp = {1., 0., 0.};
+    }
+    const auto a = Dot3(e1, temp);
+
+    // Construct e2 orthogonal to e1 and lying in the y-z plane
+    Array_3 e2 = UnitVector({temp[0] - e1[0] * a, temp[1] - e1[1] * a, temp[2] - e1[2] * a});
+
+    // Construct e3 as cross product
+    const auto e3 = CrossProduct(e1, e2);
+
+    auto q_tan = RotationMatrixToQuaternion({{
+        {e1[0], e2[0], e3[0]},
+        {e1[1], e2[1], e3[1]},
+        {e1[2], e2[2], e3[2]},
+    }});
+
+    const auto twist_rad = twist * M_PI / 180.;
+    auto q_twist =
+        RotationVectorToQuaternion({e1[0] * twist_rad, e1[1] * twist_rad, e1[2] * twist_rad});
+
+    return QuaternionCompose(q_twist, q_tan);
+}
+
 }  // namespace openturbine

src/math/vector_operations.hpp

@@ -72,4 +72,9 @@ constexpr Array_3 UnitVector(const Array_3& v) {
     };
 }
 
+/// Dot3 returns the dot product of two 3-component vectors
+constexpr double Dot3(const Array_3& v1, const Array_3& v2) {
+    return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
+}
+
 }  // namespace openturbine

tests/unit_tests/math/test_quaternion_operations.cpp

@@ -78,6 +78,23 @@ TEST(QuaternionTest, ConvertQuaternionToRotationMatrix_90DegreeRotationAboutZAxi
     }
 }
 
+TEST(QuaternionTest, ConvertRotationMatrixToQuaternion) {
+    const auto n = 25U;
+    const auto dtheta = M_PI / static_cast<double>(n);
+    for (size_t i = 0; i < n; ++i) {
+        for (size_t j = 0; j < n; ++j) {
+            auto q_ref = RotationVectorToQuaternion(
+                {static_cast<double>(i) * dtheta, static_cast<double>(j) * dtheta, 0.}
+            );
+            auto r = QuaternionToRotationMatrix(q_ref);
+            auto q_new = RotationMatrixToQuaternion(r);
+            for (auto m = 0U; m < 4; ++m) {
+                EXPECT_NEAR(q_ref[m], q_new[m], 1e-12);
+            }
+        }
+    }
+}
+
 Kokkos::View<double[3]> TestRotateVectorByQuaternion(
     const Kokkos::View<double[4]>::const_type& q, const Kokkos::View<double[3]>::const_type& v
 ) {
@@ -380,4 +397,36 @@ TEST(QuaternionTest, QuaternionToRotationVector_2) {
     }
 }
 
+TEST(QuaternionTest, CheckTangentTwistToQuaternion) {
+    struct TestData {
+        double twist;
+        Array_3 tan;
+        Array_4 q_exp;
+    };
+    for (const auto& td : std::vector<TestData>{
+             {
+                 45.,
+                 {1., 0., 0.},
+                 {0.92387953251128674, 0.38268343236508978, 0., 0.},
+             },
+             {
+                 180.,
+                 {1., 1., 0.},
+                 {0., 0.92387953251128685, 0.38268343236508978, 0.},
+             },
+             {
+                 45.,
+                 {0., 0., 1.},
+                 {0.65328148243818829, 0.27059805007309845, -0.65328148243818818, 0.27059805007309851
+                 },
+             },
+         }) {
+        const auto q_act = TangentTwistToQuaternion(td.tan, td.twist);
+        ASSERT_NEAR(q_act[0], td.q_exp[0], 1e-14);
+        ASSERT_NEAR(q_act[1], td.q_exp[1], 1e-14);
+        ASSERT_NEAR(q_act[2], td.q_exp[2], 1e-14);
+        ASSERT_NEAR(q_act[3], td.q_exp[3], 1e-14);
+    }
+}
+
 }  // namespace openturbine::tests