From f9a4b82eddfe386f39d1440a8f6c3653390ebe3f Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 10 Aug 2015 14:47:15 -0700 Subject: [PATCH] add swing-twist decomposition util with unit-tests --- libraries/shared/src/GeometryUtil.cpp | 86 ++++++++++++++------------ libraries/shared/src/GeometryUtil.h | 40 +++++++----- tests/shared/src/GeometryUtilTests.cpp | 81 +++++++++++++++++++++--- tests/shared/src/GeometryUtilTests.h | 1 + 4 files changed, 145 insertions(+), 63 deletions(-) diff --git a/libraries/shared/src/GeometryUtil.cpp b/libraries/shared/src/GeometryUtil.cpp index b612fe0696..acc112bfd6 100644 --- a/libraries/shared/src/GeometryUtil.cpp +++ b/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, - const glm::vec3& diskCenter, float diskRadius, float diskThickness, const glm::vec3& diskNormal, +bool findSphereDiskPenetration(const glm::vec3& sphereCenter, float sphereRadius, + 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. - vertexArrayA[0] = vertexArrayB[0]; + // The first vertex should be copied as is. + 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); +} diff --git a/libraries/shared/src/GeometryUtil.h b/libraries/shared/src/GeometryUtil.h index 657a06a604..b6bbdeaebb 100644 --- a/libraries/shared/src/GeometryUtil.h +++ b/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, - const glm::vec3& diskCenter, float diskRadius, float diskThickness, const glm::vec3& diskNormal, +bool findSphereDiskPenetration(const glm::vec3& sphereCenter, float sphereRadius, + 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); diff --git a/tests/shared/src/GeometryUtilTests.cpp b/tests/shared/src/GeometryUtilTests.cpp index 798951e304..44a540b478 100644 --- a/tests/shared/src/GeometryUtilTests.cpp +++ b/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); + } + } + } +} + + diff --git a/tests/shared/src/GeometryUtilTests.h b/tests/shared/src/GeometryUtilTests.h index d75fce556e..6996c8bcea 100644 --- a/tests/shared/src/GeometryUtilTests.h +++ b/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);