add swing-twist decomposition util with unit-tests

libraries/shared/src/GeometryUtil.cpp

@@ -537,3 +537,13 @@ bool findRayRectangleIntersection(const glm::vec3& origin, const glm::vec3& dire
 
     return false;
 }
+
+void swingTwistDecomposition(const glm::quat& rotation,
+        const glm::vec3& direction, // must be normalized
+        glm::quat& swing,
+        glm::quat& twist) {
+    glm::vec3 axis(rotation.x, rotation.y, rotation.z);
+    glm::vec3 twistPart = glm::dot(direction, axis) * direction;
+    twist = glm::normalize(glm::quat(rotation.w, twistPart.x, twistPart.y, twistPart.z));
+    swing = rotation * glm::inverse(twist);
+}

libraries/shared/src/GeometryUtil.h

@@ -82,6 +82,16 @@ bool findRayRectangleIntersection(const glm::vec3& origin, const glm::vec3& dire
 bool findRayTriangleIntersection(const glm::vec3& origin, const glm::vec3& direction,
                                     const glm::vec3& v0, const glm::vec3& v1, const glm::vec3& v2, float& distance);
 
+/// \brief decomposes rotation into its components such that: rotation = swing * twist
+/// \param rotation[in] rotation to decompose
+/// \param direction[in] normalized axis about which the twist happens (typically original direction before rotation applied)
+/// \param swing[out] the swing part of rotation
+/// \param twist[out] the twist part of rotation
+void swingTwistDecomposition(const glm::quat& rotation,
+        const glm::vec3& direction, // must be normalized
+        glm::quat& swing,
+        glm::quat& twist);
+
 class Triangle {
 public:
     glm::vec3 v0;

tests/shared/src/GeometryUtilTests.cpp

@@ -41,11 +41,9 @@ void GeometryUtilTests::testLocalRayRectangleIntersection() {
     glm::vec3 rectCenter(0.0f, 0.0f, 0.0f);
     glm::quat rectRotation = glm::quat(); // identity
 
-    // create points for generating rays that hit the plane and don't
-    glm::vec3 rayStart(1.0f, 2.0f, 3.0f);
-    float delta = 0.1f;
-
     { // verify hit
+        glm::vec3 rayStart(1.0f, 2.0f, 3.0f);
+        float delta = 0.1f;
         glm::vec3 rayEnd = rectCenter + rectRotation * ((0.5f * rectDimensions.x - delta) * xAxis);
         glm::vec3 rayHitDirection = glm::normalize(rayEnd - rayStart);
         float expectedDistance = glm::length(rayEnd - rayStart);
@@ -57,6 +55,8 @@ void GeometryUtilTests::testLocalRayRectangleIntersection() {
     }
 
     { // verify miss
+        glm::vec3 rayStart(1.0f, 2.0f, 3.0f);
+        float delta = 0.1f;
         glm::vec3 rayEnd = rectCenter + rectRotation * ((0.5f * rectDimensions.y + delta) * yAxis);
         glm::vec3 rayMissDirection = glm::normalize(rayEnd - rayStart);
         float distance = FLT_MAX;
@@ -67,9 +67,9 @@ void GeometryUtilTests::testLocalRayRectangleIntersection() {
 
     { // hit with co-planer line
         float yFraction = 0.25f;
-        rayStart = rectCenter + rectRotation * (rectDimensions.x * xAxis + yFraction * rectDimensions.y * yAxis);
+        glm::vec3 rayStart = rectCenter + rectRotation * (rectDimensions.x * xAxis + yFraction * rectDimensions.y * yAxis);
 
-        glm::vec3 rayEnd = rectCenter - rectRotation * (rectDimensions.x * xAxis + yFraction * rectDimensions.y * yAxis);
+        glm::vec3 rayEnd = rectCenter - rectRotation * (rectDimensions.x * xAxis - yFraction * rectDimensions.y * yAxis);
         glm::vec3 rayHitDirection = glm::normalize(rayEnd - rayStart);
         float expectedDistance = rectDimensions.x;
 
@@ -81,9 +81,9 @@ void GeometryUtilTests::testLocalRayRectangleIntersection() {
 
     { // miss with co-planer line
         float yFraction = 0.75f;
-        rayStart = rectCenter + rectRotation * (rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis);
+        glm::vec3 rayStart = rectCenter + rectRotation * (rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis);
 
-        glm::vec3 rayEnd = rectCenter + rectRotation * (- rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis);
+        glm::vec3 rayEnd = rectCenter - rectRotation * (rectDimensions.x * xAxis - (yFraction * rectDimensions.y) * yAxis);
         glm::vec3 rayHitDirection = glm::normalize(rayEnd - rayStart);
 
         float distance = FLT_MAX;
@@ -134,7 +134,7 @@ void GeometryUtilTests::testWorldRayRectangleIntersection() {
         float yFraction = 0.25f;
         rayStart = rectCenter + rectRotation * (rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis);
 
-        glm::vec3 rayEnd = rectCenter - rectRotation * (rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis);
+        glm::vec3 rayEnd = rectCenter - rectRotation * (rectDimensions.x * xAxis - (yFraction * rectDimensions.y) * yAxis);
         glm::vec3 rayHitDirection = glm::normalize(rayEnd - rayStart);
         float expectedDistance = rectDimensions.x;
 
@@ -148,7 +148,7 @@ void GeometryUtilTests::testWorldRayRectangleIntersection() {
         float yFraction = 0.75f;
         rayStart = rectCenter + rectRotation * (rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis);
 
-        glm::vec3 rayEnd = rectCenter + rectRotation * (-rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis);
+        glm::vec3 rayEnd = rectCenter - rectRotation * (rectDimensions.x * xAxis - (yFraction * rectDimensions.y) * yAxis);
         glm::vec3 rayHitDirection = glm::normalize(rayEnd - rayStart);
 
         float distance = FLT_MAX;
@@ -158,3 +158,64 @@ void GeometryUtilTests::testWorldRayRectangleIntersection() {
     }
 }
 
+void GeometryUtilTests::testTwistSwingDecomposition() {
+    // for each twist and swing angle pair:
+    // (a) compute twist and swing input components
+    // (b) compose the total rotation
+    // (c) decompose the total rotation
+    // (d) compare decomposed values with input components
+
+    glm::vec3 xAxis(1.0f, 0.0f, 0.0f);
+    glm::vec3 twistAxis = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f)); // can be anything but xAxis
+    glm::vec3 initialSwingAxis = glm::normalize(glm::cross(xAxis, twistAxis)); // initialSwingAxis must be perp to twistAxis
+
+    const int numTwists = 6;
+    const int numSwings = 7;
+    const int numSwingAxes = 5;
+
+    const float smallAngle = PI / 100.0f;
+
+    const float maxTwist = PI;
+    const float minTwist = -PI;
+    const float minSwing = 0.0f;
+    const float maxSwing = PI;
+
+    const float deltaTwist = (maxTwist - minTwist - 2.0f * smallAngle) / (float)(numTwists - 1);
+    const float deltaSwing = (maxSwing - minSwing - 2.0f * smallAngle) / (float)(numSwings - 1);
+
+    for (float twist = minTwist + smallAngle; twist < maxTwist; twist += deltaTwist) {
+        // compute twist component
+        glm::quat twistRotation = glm::angleAxis(twist, twistAxis);
+
+        float deltaTheta = TWO_PI / (numSwingAxes - 1);
+        for (float theta = 0.0f; theta < TWO_PI; theta += deltaTheta) {
+            // compute the swingAxis
+            glm::quat thetaRotation = glm::angleAxis(theta, twistAxis);
+            glm::vec3 swingAxis = thetaRotation * initialSwingAxis;
+
+            for (float swing = minSwing + smallAngle; swing < maxSwing; swing += deltaSwing) {
+                // compute swing component
+                glm::quat swingRotation = glm::angleAxis(swing, swingAxis);
+
+                // compose
+                glm::quat totalRotation = swingRotation * twistRotation;
+
+                // decompose
+                glm::quat measuredTwistRotation;
+                glm::quat measuredSwingRotation;
+                swingTwistDecomposition(totalRotation, twistAxis, measuredSwingRotation, measuredTwistRotation);
+    
+                // dot decomposed with components
+                float twistDot = fabsf(glm::dot(twistRotation, measuredTwistRotation));
+                float swingDot = fabsf(glm::dot(swingRotation, measuredSwingRotation));
+    
+                // the dot products should be very close to 1.0
+                const float MIN_ERROR = 1.0e-6f;
+                QCOMPARE_WITH_ABS_ERROR(1.0f, twistDot, MIN_ERROR);
+                QCOMPARE_WITH_ABS_ERROR(1.0f, swingDot, MIN_ERROR);
+            }
+        }
+    }
+}
+
+

tests/shared/src/GeometryUtilTests.h

@@ -20,6 +20,7 @@ class GeometryUtilTests : public QObject {
 private slots:
     void testLocalRayRectangleIntersection();
     void testWorldRayRectangleIntersection();
+    void testTwistSwingDecomposition();
 };
 
 float getErrorDifference(const float& a, const float& b);