add swing-twist decomposition util with unit-tests

libraries/shared/src/GeometryUtil.cpp

@@ -26,20 +26,20 @@ glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const glm::vec
     float proj = glm::dot(point - start, segmentVector) / lengthSquared;
     if (proj <= 0.0f) { // closest to the start
         return start - point;
-        
+
     } else if (proj >= 1.0f) { // closest to the end
         return end - point;
-    
+
     } else { // closest to the middle
         return start + segmentVector*proj - point;
     }
 }
 
 // Computes the penetration between a point and a sphere (centered at the origin)
-// if point is inside sphere: returns true and stores the result in 'penetration' 
+// if point is inside sphere: returns true and stores the result in 'penetration'
 // (the vector that would move the point outside the sphere)
 // otherwise returns false
-bool findSpherePenetration(const glm::vec3& point, const glm::vec3& defaultDirection, float sphereRadius, 
+bool findSpherePenetration(const glm::vec3& point, const glm::vec3& defaultDirection, float sphereRadius,
                            glm::vec3& penetration) {
     float vectorLength = glm::length(point);
     if (vectorLength < EPSILON) {
@@ -71,7 +71,7 @@ bool findSphereSpherePenetration(const glm::vec3& firstCenter, float firstRadius
 
 bool findSphereSegmentPenetration(const glm::vec3& sphereCenter, float sphereRadius,
                                   const glm::vec3& segmentStart, const glm::vec3& segmentEnd, glm::vec3& penetration) {
-    return findSpherePenetration(computeVectorFromPointToSegment(sphereCenter, segmentStart, segmentEnd), 
+    return findSpherePenetration(computeVectorFromPointToSegment(sphereCenter, segmentStart, segmentEnd),
                                  glm::vec3(0.0f, -1.0f, 0.0f), sphereRadius, penetration);
 }
 
@@ -93,10 +93,10 @@ bool findPointCapsuleConePenetration(const glm::vec3& point, const glm::vec3& ca
     float proj = glm::dot(point - capsuleStart, segmentVector) / lengthSquared;
     if (proj <= 0.0f) { // closest to the start
         return findPointSpherePenetration(point, capsuleStart, startRadius, penetration);
-        
+
     } else if (proj >= 1.0f) { // closest to the end
         return findPointSpherePenetration(point, capsuleEnd, endRadius, penetration);
-    
+
     } else { // closest to the middle
         return findPointSpherePenetration(point, capsuleStart + segmentVector * proj,
             glm::mix(startRadius, endRadius, proj), penetration);
@@ -110,7 +110,7 @@ bool findSphereCapsuleConePenetration(const glm::vec3& sphereCenter,
         startRadius + sphereRadius, endRadius + sphereRadius, penetration);
 }
 
-bool findSpherePlanePenetration(const glm::vec3& sphereCenter, float sphereRadius, 
+bool findSpherePlanePenetration(const glm::vec3& sphereCenter, float sphereRadius,
                                 const glm::vec4& plane, glm::vec3& penetration) {
     float distance = glm::dot(plane, glm::vec4(sphereCenter, 1.0f)) - sphereRadius;
     if (distance < 0.0f) {
@@ -120,8 +120,8 @@ bool findSpherePlanePenetration(const glm::vec3& sphereCenter, float sphereRadiu
     return false;
 }
 
-bool findSphereDiskPenetration(const glm::vec3& sphereCenter, float sphereRadius, 
+bool findSphereDiskPenetration(const glm::vec3& sphereCenter, float sphereRadius,
-                               const glm::vec3& diskCenter, float diskRadius, float diskThickness, const glm::vec3& diskNormal, 
+                               const glm::vec3& diskCenter, float diskRadius, float diskThickness, const glm::vec3& diskNormal,
                                glm::vec3& penetration) {
     glm::vec3 localCenter = sphereCenter - diskCenter;
     float axialDistance = glm::dot(localCenter, diskNormal);
@@ -171,12 +171,12 @@ glm::vec3 addPenetrations(const glm::vec3& currentPenetration, const glm::vec3&
     }
     glm::vec3 currentDirection = currentPenetration / currentLength;
     float directionalComponent = glm::dot(newPenetration, currentDirection);
-    
+
     // if orthogonal or in the opposite direction, we can simply add
     if (directionalComponent <= 0.0f) {
         return currentPenetration + newPenetration;
     }
-    
+
     // otherwise, we need to take the maximum component of current and new
     return currentDirection * glm::max(directionalComponent, currentLength) +
         newPenetration - (currentDirection * directionalComponent);
@@ -217,14 +217,14 @@ bool findRayCapsuleIntersection(const glm::vec3& origin, const glm::vec3& direct
     float c = glm::dot(constant, constant) - radius * radius;
     if (c < 0.0f) { // starts inside cylinder
         if (originProjection < 0.0f) { // below start
-            return findRaySphereIntersection(origin, direction, start, radius, distance); 
+            return findRaySphereIntersection(origin, direction, start, radius, distance);
-            
+
         } else if (originProjection > capsuleLength) { // above end
-            return findRaySphereIntersection(origin, direction, end, radius, distance); 
+            return findRaySphereIntersection(origin, direction, end, radius, distance);
-        
+
         } else { // between start and end
             distance = 0.0f;
-            return true; 
+            return true;
         }
     }
     glm::vec3 coefficient = direction - relativeEnd * glm::dot(relativeEnd, direction);
@@ -245,10 +245,10 @@ bool findRayCapsuleIntersection(const glm::vec3& origin, const glm::vec3& direct
     float intersectionProjection = glm::dot(relativeEnd, intersection);
     if (intersectionProjection < 0.0f) { // below start
         return findRaySphereIntersection(origin, direction, start, radius, distance);
-        
+
     } else if (intersectionProjection > capsuleLength) { // above end
         return findRaySphereIntersection(origin, direction, end, radius, distance);
-    } 
+    }
     distance = t; // between start and end
     return true;
 }
@@ -311,7 +311,7 @@ int computeDirection(float xi, float yi, float xj, float yj, float xk, float yk)
 //
 //         (0,0)                   (windowWidth, 0)
 //         -1,1                    1,1
-//           +-----------------------+ 
+//           +-----------------------+
 //           |           |           |
 //           |           |           |
 //           | -1,0      |           |
@@ -341,10 +341,10 @@ void PolygonClip::clipToScreen(const glm::vec2* inputVertexArray, int inLength,
     int maxLength = inLength * 2;
     glm::vec2* tempVertexArrayA = new glm::vec2[maxLength];
     glm::vec2* tempVertexArrayB = new glm::vec2[maxLength];
-    
+
     // set up our temporary arrays
     memcpy(tempVertexArrayA, inputVertexArray, sizeof(glm::vec2) * inLength);
-    
+
     // Left edge
     LineSegment2 edge;
     edge[0] = TOP_LEFT_CLIPPING_WINDOW;
@@ -353,7 +353,7 @@ void PolygonClip::clipToScreen(const glm::vec2* inputVertexArray, int inLength,
     sutherlandHodgmanPolygonClip(tempVertexArrayA, tempVertexArrayB, tempLengthA, tempLengthB, edge);
     // clean the array from tempVertexArrayA and copy cleaned result to tempVertexArrayA
     copyCleanArray(tempLengthA, tempVertexArrayA, tempLengthB, tempVertexArrayB);
-    
+
     // Bottom Edge
     edge[0] = BOTTOM_LEFT_CLIPPING_WINDOW;
     edge[1] = BOTTOM_RIGHT_CLIPPING_WINDOW;
@@ -361,7 +361,7 @@ void PolygonClip::clipToScreen(const glm::vec2* inputVertexArray, int inLength,
     sutherlandHodgmanPolygonClip(tempVertexArrayA, tempVertexArrayB, tempLengthA, tempLengthB, edge);
     // clean the array from tempVertexArrayA and copy cleaned result to tempVertexArrayA
     copyCleanArray(tempLengthA, tempVertexArrayA, tempLengthB, tempVertexArrayB);
-    
+
     // Right Edge
     edge[0] = BOTTOM_RIGHT_CLIPPING_WINDOW;
     edge[1] = TOP_RIGHT_CLIPPING_WINDOW;
@@ -369,7 +369,7 @@ void PolygonClip::clipToScreen(const glm::vec2* inputVertexArray, int inLength,
     sutherlandHodgmanPolygonClip(tempVertexArrayA, tempVertexArrayB, tempLengthA, tempLengthB, edge);
     // clean the array from tempVertexArrayA and copy cleaned result to tempVertexArrayA
     copyCleanArray(tempLengthA, tempVertexArrayA, tempLengthB, tempVertexArrayB);
-    
+
     // Top Edge
     edge[0] = TOP_RIGHT_CLIPPING_WINDOW;
     edge[1] = TOP_LEFT_CLIPPING_WINDOW;
@@ -377,18 +377,18 @@ void PolygonClip::clipToScreen(const glm::vec2* inputVertexArray, int inLength,
     sutherlandHodgmanPolygonClip(tempVertexArrayA, tempVertexArrayB, tempLengthA, tempLengthB, edge);
     // clean the array from tempVertexArrayA and copy cleaned result to tempVertexArrayA
     copyCleanArray(tempLengthA, tempVertexArrayA, tempLengthB, tempVertexArrayB);
-    
+
     // copy final output to outputVertexArray
     outputVertexArray = tempVertexArrayA;
     outLength         = tempLengthA;
 
     // cleanup our unused temporary buffer...
     delete[] tempVertexArrayB;
-    
+
     // Note: we don't delete tempVertexArrayA, because that's the caller's responsibility
 }
 
-void PolygonClip::sutherlandHodgmanPolygonClip(glm::vec2* inVertexArray, glm::vec2* outVertexArray, 
+void PolygonClip::sutherlandHodgmanPolygonClip(glm::vec2* inVertexArray, glm::vec2* outVertexArray,
                                                int inLength, int& outLength,  const LineSegment2& clipBoundary) {
     glm::vec2 start, end; // Start, end point of current polygon edge
     glm::vec2 intersection;   // Intersection point with a clip boundary
@@ -397,8 +397,8 @@ void PolygonClip::sutherlandHodgmanPolygonClip(glm::vec2* inVertexArray, glm::ve
     start = inVertexArray[inLength - 1]; // Start with the last vertex in inVertexArray
     for (int j = 0; j < inLength; j++) {
         end = inVertexArray[j]; // Now start and end correspond to the vertices
-        
+
-        // Cases 1 and 4 - the endpoint is inside the boundary 
+        // Cases 1 and 4 - the endpoint is inside the boundary
         if (pointInsideBoundary(end,clipBoundary)) {
             // Case 1 - Both inside
             if (pointInsideBoundary(start, clipBoundary)) {
@@ -409,14 +409,14 @@ void PolygonClip::sutherlandHodgmanPolygonClip(glm::vec2* inVertexArray, glm::ve
                 appendPoint(end, outLength, outVertexArray);
             }
         } else { // Cases 2 and 3 - end is outside
-            if (pointInsideBoundary(start, clipBoundary))  { 
+            if (pointInsideBoundary(start, clipBoundary))  {
                 // Cases 2 - start is inside, end is outside
                 segmentIntersectsBoundary(start, end, clipBoundary, intersection);
                 appendPoint(intersection, outLength, outVertexArray);
             } else {
                 // Case 3 - both are outside, No action
             }
-       } 
+       }
        start = end;  // Advance to next pair of vertices
     }
 }
@@ -468,23 +468,23 @@ void PolygonClip::appendPoint(glm::vec2 newVertex, int& outLength, glm::vec2* ou
 }
 
 // The copyCleanArray() function sets the resulting polygon of the previous step up to be the input polygon for next step of the
-// clipping algorithm. As the Sutherland-Hodgman algorithm is a polygon clipping algorithm, it does not handle line 
+// clipping algorithm. As the Sutherland-Hodgman algorithm is a polygon clipping algorithm, it does not handle line
 // clipping very well. The modification so that lines may be clipped as well as polygons is included in this function.
-// when completed vertexArrayA will be ready for output and/or next step of clipping 
+// when completed vertexArrayA will be ready for output and/or next step of clipping
 void PolygonClip::copyCleanArray(int& lengthA, glm::vec2* vertexArrayA, int& lengthB, glm::vec2* vertexArrayB) {
     // Fix lines: they will come back with a length of 3, from an original of length of 2
     if ((lengthA == 2) && (lengthB == 3)) {
-        // The first vertex should be copied as is. 
+        // The first vertex should be copied as is.
-        vertexArrayA[0] = vertexArrayB[0]; 
+        vertexArrayA[0] = vertexArrayB[0];
         // If the first two vertices of the "B" array are same, then collapse them down to be the 2nd vertex
         if (vertexArrayB[0].x == vertexArrayB[1].x)  {
             vertexArrayA[1] = vertexArrayB[2];
-        } else { 
+        } else {
             // Otherwise the first vertex should be the same as third vertex
             vertexArrayA[1] = vertexArrayB[1];
         }
         lengthA=2;
-    } else { 
+    } else {
         // for all other polygons, then just copy the vertexArrayB to vertextArrayA for next step
         lengthA = lengthB;
         for (int i = 0; i < lengthB; i++) {
@@ -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

@@ -22,7 +22,7 @@ glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const glm::vec
 /// \param sphereRadius the radius of the sphere
 /// \param penetration[out] the displacement that would move the point out of penetration with the sphere
 /// \return true if point is inside sphere, otherwise false
-bool findSpherePenetration(const glm::vec3& point, const glm::vec3& defaultDirection, 
+bool findSpherePenetration(const glm::vec3& point, const glm::vec3& defaultDirection,
                            float sphereRadius, glm::vec3& penetration);
 
 bool findSpherePointPenetration(const glm::vec3& sphereCenter, float sphereRadius,
@@ -33,7 +33,7 @@ bool findPointSpherePenetration(const glm::vec3& point, const glm::vec3& sphereC
 
 bool findSphereSpherePenetration(const glm::vec3& firstCenter, float firstRadius,
                                  const glm::vec3& secondCenter, float secondRadius, glm::vec3& penetration);
-                     
+
 bool findSphereSegmentPenetration(const glm::vec3& sphereCenter, float sphereRadius,
                                   const glm::vec3& segmentStart, const glm::vec3& segmentEnd, glm::vec3& penetration);
 
@@ -42,14 +42,14 @@ bool findSphereCapsulePenetration(const glm::vec3& sphereCenter, float sphereRad
 
 bool findPointCapsuleConePenetration(const glm::vec3& point, const glm::vec3& capsuleStart,
     const glm::vec3& capsuleEnd, float startRadius, float endRadius, glm::vec3& penetration);
-    
+
-bool findSphereCapsuleConePenetration(const glm::vec3& sphereCenter, float sphereRadius, 
+bool findSphereCapsuleConePenetration(const glm::vec3& sphereCenter, float sphereRadius,
     const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd,
     float startRadius, float endRadius, glm::vec3& penetration);
-                                  
+
-bool findSpherePlanePenetration(const glm::vec3& sphereCenter, float sphereRadius, 
+bool findSpherePlanePenetration(const glm::vec3& sphereCenter, float sphereRadius,
                                 const glm::vec4& plane, glm::vec3& penetration);
-                                  
+
 /// Computes the penetration between a sphere and a disk.
 /// \param sphereCenter center of sphere
 /// \param sphereRadius radius of sphere
@@ -58,8 +58,8 @@ bool findSpherePlanePenetration(const glm::vec3& sphereCenter, float sphereRadiu
 /// \param diskNormal normal of disk plan
 /// \param penetration[out] the depth that the sphere penetrates the disk
 /// \return true if sphere touches disk (does not handle collisions with disk edge)
-bool findSphereDiskPenetration(const glm::vec3& sphereCenter, float sphereRadius, 
+bool findSphereDiskPenetration(const glm::vec3& sphereCenter, float sphereRadius,
-                               const glm::vec3& diskCenter, float diskRadius, float diskThickness, const glm::vec3& diskNormal, 
+                               const glm::vec3& diskCenter, float diskRadius, float diskThickness, const glm::vec3& diskNormal,
                                glm::vec3& penetration);
 
 bool findCapsuleSpherePenetration(const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius,
@@ -79,9 +79,19 @@ bool findRayCapsuleIntersection(const glm::vec3& origin, const glm::vec3& direct
 bool findRayRectangleIntersection(const glm::vec3& origin, const glm::vec3& direction, const glm::quat& rotation,
         const glm::vec3& position, const glm::vec2& dimensions, float& distance);
 
-bool findRayTriangleIntersection(const glm::vec3& origin, const glm::vec3& direction, 
+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;
@@ -89,11 +99,11 @@ public:
     glm::vec3 v2;
 };
 
-inline bool findRayTriangleIntersection(const glm::vec3& origin, const glm::vec3& direction, 
+inline bool findRayTriangleIntersection(const glm::vec3& origin, const glm::vec3& direction,
                                     const Triangle& triangle, float& distance) {
     return findRayTriangleIntersection(origin, direction, triangle.v0, triangle.v1, triangle.v2, distance);
 }
-    
+
 
 bool doLineSegmentsIntersect(glm::vec2 r1p1, glm::vec2 r1p2, glm::vec2 r2p1, glm::vec2 r2p2);
 bool isOnSegment(float xi, float yi, float xj, float yj, float xk, float yk);
@@ -117,15 +127,15 @@ public:
     static const glm::vec2 TOP_RIGHT_CLIPPING_WINDOW;
     static const glm::vec2 BOTTOM_LEFT_CLIPPING_WINDOW;
     static const glm::vec2 BOTTOM_RIGHT_CLIPPING_WINDOW;
-    
+
 private:
 
-    static void sutherlandHodgmanPolygonClip(glm::vec2* inVertexArray, glm::vec2* outVertexArray, 
+    static void sutherlandHodgmanPolygonClip(glm::vec2* inVertexArray, glm::vec2* outVertexArray,
                                              int inLength, int& outLength,  const LineSegment2& clipBoundary);
 
     static bool pointInsideBoundary(const glm::vec2& testVertex, const LineSegment2& clipBoundary);
 
-    static void segmentIntersectsBoundary(const glm::vec2& first, const glm::vec2&  second, 
+    static void segmentIntersectsBoundary(const glm::vec2& first, const glm::vec2&  second,
                                           const LineSegment2& clipBoundary, glm::vec2& intersection);
 
     static void appendPoint(glm::vec2 newVertex, int& outLength, glm::vec2* outVertexArray);

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);