From 6db47e773f4bc85aec0d59299b6417a4dcbbf425 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 7 Aug 2015 11:58:26 -0700 Subject: [PATCH 01/10] removing ShapeColliderTests.* --- tests/physics/src/ShapeColliderTests.cpp | 1936 ---------------------- tests/physics/src/ShapeColliderTests.h | 53 - 2 files changed, 1989 deletions(-) delete mode 100644 tests/physics/src/ShapeColliderTests.cpp delete mode 100644 tests/physics/src/ShapeColliderTests.h diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp deleted file mode 100644 index cb51e18fbd..0000000000 --- a/tests/physics/src/ShapeColliderTests.cpp +++ /dev/null @@ -1,1936 +0,0 @@ -// -// ShapeColliderTests.cpp -// tests/physics/src -// -// Created by Andrew Meadows on 02/21/2014. -// Copyright 2014 High Fidelity, Inc. -// -// Distributed under the Apache License, Version 2.0. -// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html -// - -#include "ShapeColliderTests.h" - -//#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -// Add additional qtest functionality (the include order is important!) -#include "BulletTestUtils.h" -#include "GlmTestUtils.h" -#include "../QTestExtensions.h" - - -const glm::vec3 origin(0.0f); -static const glm::vec3 xAxis(1.0f, 0.0f, 0.0f); -static const glm::vec3 yAxis(0.0f, 1.0f, 0.0f); -static const glm::vec3 zAxis(0.0f, 0.0f, 1.0f); - -QTEST_MAIN(ShapeColliderTests) - -void ShapeColliderTests::initTestCase() { - ShapeCollider::initDispatchTable(); -} - -void ShapeColliderTests::sphereMissesSphere() { - // non-overlapping spheres of unequal size - float radiusA = 7.0f; - float radiusB = 3.0f; - float alpha = 1.2f; - float beta = 1.3f; - glm::vec3 offsetDirection = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f)); - float offsetDistance = alpha * radiusA + beta * radiusB; - - SphereShape sphereA(radiusA, origin); - SphereShape sphereB(radiusB, offsetDistance * offsetDirection); - CollisionList collisions(16); - - // collide A to B and vice versa - QCOMPARE(ShapeCollider::collideShapes(&sphereA, &sphereB, collisions), false); - QCOMPARE(ShapeCollider::collideShapes(&sphereB, &sphereA, collisions), false); - - // Collision list should be empty - QCOMPARE(collisions.size(), 0); -} - -void ShapeColliderTests::sphereTouchesSphere() { - // overlapping spheres of unequal size - float radiusA = 7.0f; - float radiusB = 3.0f; - float alpha = 0.2f; - float beta = 0.3f; - glm::vec3 offsetDirection = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f)); - float offsetDistance = alpha * radiusA + beta * radiusB; - float expectedPenetrationDistance = (1.0f - alpha) * radiusA + (1.0f - beta) * radiusB; - glm::vec3 expectedPenetration = expectedPenetrationDistance * offsetDirection; - - SphereShape sphereA(radiusA, origin); - SphereShape sphereB(radiusB, offsetDistance * offsetDirection); - CollisionList collisions(16); - int numCollisions = 0; - - // collide A to B... - { - QCOMPARE(ShapeCollider::collideShapes(&sphereA, &sphereB, collisions), true); - ++numCollisions; - - // verify state of collisions - QCOMPARE(collisions.size(), numCollisions); - CollisionInfo* collision = collisions.getCollision(numCollisions - 1); - QVERIFY(collision != nullptr); - - // penetration points from sphereA into sphereB - QCOMPARE(collision->_penetration, expectedPenetration); - - // contactPoint is on surface of sphereA - glm::vec3 AtoB = sphereB.getTranslation() - sphereA.getTranslation(); - glm::vec3 expectedContactPoint = sphereA.getTranslation() + radiusA * glm::normalize(AtoB); - QCOMPARE(collision->_contactPoint, expectedContactPoint); - - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, EPSILON); - } - - // collide B to A... - { - QCOMPARE(ShapeCollider::collideShapes(&sphereB, &sphereA, collisions), true); - ++numCollisions; - - // penetration points from sphereA into sphereB - CollisionInfo* collision = collisions.getCollision(numCollisions - 1); - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, -expectedPenetration, EPSILON); - - // contactPoint is on surface of sphereB - glm::vec3 BtoA = sphereA.getTranslation() - sphereB.getTranslation(); - glm::vec3 expectedContactPoint = sphereB.getTranslation() + radiusB * glm::normalize(BtoA); - - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, EPSILON); - } -} - -void ShapeColliderTests::sphereMissesCapsule() { - // non-overlapping sphere and capsule - float radiusA = 1.5f; - float radiusB = 2.3f; - float totalRadius = radiusA + radiusB; - float halfHeightB = 1.7f; - float axialOffset = totalRadius + 1.1f * halfHeightB; - float radialOffset = 1.2f * radiusA + 1.3f * radiusB; - - SphereShape sphereA(radiusA); - CapsuleShape capsuleB(radiusB, halfHeightB); - - // give the capsule some arbirary transform - float angle = 37.8f; - glm::vec3 axis = glm::normalize( glm::vec3(-7.0f, 2.8f, 9.3f) ); - glm::quat rotation = glm::angleAxis(angle, axis); - glm::vec3 translation(15.1f, -27.1f, -38.6f); - capsuleB.setRotation(rotation); - capsuleB.setTranslation(translation); - - CollisionList collisions(16); - - // walk sphereA along the local yAxis next to, but not touching, capsuleB - glm::vec3 localStartPosition(radialOffset, axialOffset, 0.0f); - int numberOfSteps = 10; - float delta = 1.3f * (totalRadius + halfHeightB) / (numberOfSteps - 1); - for (int i = 0; i < numberOfSteps; ++i) { - // translate sphereA into world-frame - glm::vec3 localPosition = localStartPosition + ((float)i * delta) * yAxis; - sphereA.setTranslation(rotation * localPosition + translation); - - // sphereA agains capsuleB and vice versa - QCOMPARE(ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions), false); - QCOMPARE(ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions), false); - } - - QCOMPARE(collisions.size(), 0); -} - -void ShapeColliderTests::sphereTouchesCapsule() { - // overlapping sphere and capsule - float radiusA = 2.0f; - float radiusB = 1.0f; - float totalRadius = radiusA + radiusB; - float halfHeightB = 2.0f; - float alpha = 0.5f; - float beta = 0.5f; - float radialOffset = alpha * radiusA + beta * radiusB; - - SphereShape sphereA(radiusA); - CapsuleShape capsuleB(radiusB, halfHeightB); - - CollisionList collisions(16); - int numCollisions = 0; - - { // sphereA collides with capsuleB's cylindrical wall - sphereA.setTranslation(radialOffset * xAxis); - - QCOMPARE(ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions), true); - ++numCollisions; - - // penetration points from sphereA into capsuleB - CollisionInfo* collision = collisions.getCollision(numCollisions - 1); - glm::vec3 expectedPenetration = (radialOffset - totalRadius) * xAxis; - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, EPSILON); - - // contactPoint is on surface of sphereA - glm::vec3 expectedContactPoint = sphereA.getTranslation() - radiusA * xAxis; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, EPSILON); - - // capsuleB collides with sphereA - QCOMPARE(ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions), true); - ++numCollisions; - - // penetration points from sphereA into capsuleB - collision = collisions.getCollision(numCollisions - 1); - expectedPenetration = - (radialOffset - totalRadius) * xAxis; - if (collision->_shapeA == &sphereA) { - // the ShapeCollider swapped the order of the shapes - expectedPenetration *= -1.0f; - } - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, EPSILON); - - // contactPoint is on surface of capsuleB - glm::vec3 BtoA = sphereA.getTranslation() - capsuleB.getTranslation(); - glm::vec3 closestApproach = capsuleB.getTranslation() + glm::dot(BtoA, yAxis) * yAxis; - expectedContactPoint = closestApproach + radiusB * glm::normalize(BtoA - closestApproach); - if (collision->_shapeA == &sphereA) { - // the ShapeCollider swapped the order of the shapes - closestApproach = sphereA.getTranslation() - glm::dot(BtoA, yAxis) * yAxis; - expectedContactPoint = closestApproach - radiusB * glm::normalize(BtoA - closestApproach); - } - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, EPSILON); - } - { // sphereA hits end cap at axis - glm::vec3 axialOffset = (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis; - sphereA.setTranslation(axialOffset); - - QCOMPARE(ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions), true); - ++numCollisions; - - // penetration points from sphereA into capsuleB - CollisionInfo* collision = collisions.getCollision(numCollisions - 1); - glm::vec3 expectedPenetration = - ((1.0f - alpha) * radiusA + (1.0f - beta) * radiusB) * yAxis; - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, EPSILON); - - // contactPoint is on surface of sphereA - glm::vec3 expectedContactPoint = sphereA.getTranslation() - radiusA * yAxis; - - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, EPSILON); - - // capsuleB collides with sphereA - if (!ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions)) - { - std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: capsule and sphere should touch" << std::endl; - } else { - ++numCollisions; - } - - // penetration points from sphereA into capsuleB - collision = collisions.getCollision(numCollisions - 1); - expectedPenetration = ((1.0f - alpha) * radiusA + (1.0f - beta) * radiusB) * yAxis; - if (collision->_shapeA == &sphereA) { - // the ShapeCollider swapped the order of the shapes - expectedPenetration *= -1.0f; - } - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, EPSILON); - - // contactPoint is on surface of capsuleB - glm::vec3 endPoint; - capsuleB.getEndPoint(endPoint); - expectedContactPoint = endPoint + radiusB * yAxis; - if (collision->_shapeA == &sphereA) { - // the ShapeCollider swapped the order of the shapes - expectedContactPoint = axialOffset - radiusA * yAxis; - } - - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, EPSILON); - } - { // sphereA hits start cap at axis - glm::vec3 axialOffset = - (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis; - sphereA.setTranslation(axialOffset); - - if (!ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions)) - { - std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: sphere and capsule should touch" << std::endl; - } else { - ++numCollisions; - } - - // penetration points from sphereA into capsuleB - CollisionInfo* collision = collisions.getCollision(numCollisions - 1); - glm::vec3 expectedPenetration = ((1.0f - alpha) * radiusA + (1.0f - beta) * radiusB) * yAxis; - - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, EPSILON); - - // contactPoint is on surface of sphereA - glm::vec3 expectedContactPoint = sphereA.getTranslation() + radiusA * yAxis; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, EPSILON); - - // capsuleB collides with sphereA - if (!ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions)) - { - std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: capsule and sphere should touch" << std::endl; - } else { - ++numCollisions; - } - - // penetration points from sphereA into capsuleB - collision = collisions.getCollision(numCollisions - 1); - expectedPenetration = - ((1.0f - alpha) * radiusA + (1.0f - beta) * radiusB) * yAxis; - if (collision->_shapeA == &sphereA) { - // the ShapeCollider swapped the order of the shapes - expectedPenetration *= -1.0f; - } - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, EPSILON); - - // contactPoint is on surface of capsuleB - glm::vec3 startPoint; - capsuleB.getStartPoint(startPoint); - expectedContactPoint = startPoint - radiusB * yAxis; - if (collision->_shapeA == &sphereA) { - // the ShapeCollider swapped the order of the shapes - expectedContactPoint = axialOffset + radiusA * yAxis; - } - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, EPSILON); - } - if (collisions.size() != numCollisions) { - std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: expected " << numCollisions << " collisions but actual number is " << collisions.size() - << std::endl; - } -} - -void ShapeColliderTests::capsuleMissesCapsule() { - // non-overlapping capsules - float radiusA = 2.0f; - float halfHeightA = 3.0f; - float radiusB = 3.0f; - float halfHeightB = 4.0f; - - float totalRadius = radiusA + radiusB; - float totalHalfLength = totalRadius + halfHeightA + halfHeightB; - - CapsuleShape capsuleA(radiusA, halfHeightA); - CapsuleShape capsuleB(radiusB, halfHeightB); - - CollisionList collisions(16); - - // side by side - capsuleB.setTranslation((1.01f * totalRadius) * xAxis); - QCOMPARE(ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions), false); - QCOMPARE(ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions), false); - - // end to end - capsuleB.setTranslation((1.01f * totalHalfLength) * xAxis); - QCOMPARE(ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions), false); - QCOMPARE(ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions), false); - - // rotate B and move it to the side - glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis); - capsuleB.setRotation(rotation); - capsuleB.setTranslation((1.01f * (totalRadius + capsuleB.getHalfHeight())) * xAxis); - - QCOMPARE(ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions), false); - QCOMPARE(ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions), false); - - QCOMPARE(collisions.size(), 0); -} - -void ShapeColliderTests::capsuleTouchesCapsule() { - // overlapping capsules - float radiusA = 2.0f; - float halfHeightA = 3.0f; - float radiusB = 3.0f; - float halfHeightB = 4.0f; - - float totalRadius = radiusA + radiusB; - float totalHalfLength = totalRadius + halfHeightA + halfHeightB; - - CapsuleShape capsuleA(radiusA, halfHeightA); - CapsuleShape capsuleB(radiusB, halfHeightB); - - CollisionList collisions(16); - int numCollisions = 0; - - { // side by side - capsuleB.setTranslation((0.99f * totalRadius) * xAxis); - QCOMPARE(ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions), true); - QCOMPARE(ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions), true); - numCollisions += 2; - } - - { // end to end - capsuleB.setTranslation((0.99f * totalHalfLength) * yAxis); - - QCOMPARE(ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions), true); - QCOMPARE(ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions), true); - numCollisions += 2; - } - - { // rotate B and move it to the side - glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis); - capsuleB.setRotation(rotation); - capsuleB.setTranslation((0.99f * (totalRadius + capsuleB.getHalfHeight())) * xAxis); - - QCOMPARE(ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions), true); - QCOMPARE(ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions), true); - numCollisions += 2; - } - - { // again, but this time check collision details - float overlap = 0.1f; - glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis); - capsuleB.setRotation(rotation); - glm::vec3 positionB = ((totalRadius + capsuleB.getHalfHeight()) - overlap) * xAxis; - capsuleB.setTranslation(positionB); - - // capsuleA vs capsuleB - QCOMPARE(ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions), true); - ++numCollisions; - - CollisionInfo* collision = collisions.getCollision(numCollisions - 1); - glm::vec3 expectedPenetration = overlap * xAxis; - - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, EPSILON); - - glm::vec3 expectedContactPoint = capsuleA.getTranslation() + radiusA * xAxis; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, EPSILON); - - // capsuleB vs capsuleA - QCOMPARE(ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions), true); - ++numCollisions; - - collision = collisions.getCollision(numCollisions - 1); - expectedPenetration = - overlap * xAxis; - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, EPSILON); - - expectedContactPoint = capsuleB.getTranslation() - (radiusB + halfHeightB) * xAxis; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, EPSILON); - } - - { // collide cylinder wall against cylinder wall - float overlap = 0.137f; - float shift = 0.317f * halfHeightA; - glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis); - capsuleB.setRotation(rotation); - glm::vec3 positionB = (totalRadius - overlap) * zAxis + shift * yAxis; - capsuleB.setTranslation(positionB); - - // capsuleA vs capsuleB - QCOMPARE(ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions), true); - ++numCollisions; - - CollisionInfo* collision = collisions.getCollision(numCollisions - 1); - glm::vec3 expectedPenetration = overlap * zAxis; - - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, EPSILON); - - glm::vec3 expectedContactPoint = capsuleA.getTranslation() + radiusA * zAxis + shift * yAxis; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, EPSILON); - } -} - -void ShapeColliderTests::sphereMissesAACube() { - CollisionList collisions(16); - - float sphereRadius = 1.0f; - glm::vec3 sphereCenter(0.0f); - - glm::vec3 cubeCenter(1.5f, 0.0f, 0.0f); - - float cubeSide = 2.0f; - - glm::vec3 faceNormals[] = {xAxis, yAxis, zAxis}; - int numDirections = 3; - - float offset = 2.0f * EPSILON; - - // faces - for (int i = 0; i < numDirections; ++i) { - for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) { - glm::vec3 faceNormal = sign * faceNormals[i]; - - sphereCenter = cubeCenter + (0.5f * cubeSide + sphereRadius + offset) * faceNormal; - - CollisionInfo* collision = ShapeCollider::sphereVsAACubeHelper(sphereCenter, sphereRadius, - cubeCenter, cubeSide, collisions); - - if (collision) { - QFAIL_WITH_MESSAGE("sphere should NOT collide with cube face.\n\t\t" - << "faceNormal = " << faceNormal); -// std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube face." -// << " faceNormal = " << faceNormal << std::endl; - } - } - } - - // edges - int numSteps = 5; - // loop over each face... - for (int i = 0; i < numDirections; ++i) { - for (float faceSign = -1.0f; faceSign < 2.0f; faceSign += 2.0f) { - glm::vec3 faceNormal = faceSign * faceNormals[i]; - - // loop over each neighboring face... - for (int j = (i + 1) % numDirections; j != i; j = (j + 1) % numDirections) { - // Compute the index to the third direction, which points perpendicular to both the face - // and the neighbor face. - int k = (j + 1) % numDirections; - if (k == i) { - k = (i + 1) % numDirections; - } - glm::vec3 thirdNormal = faceNormals[k]; - - for (float neighborSign = -1.0f; neighborSign < 2.0f; neighborSign += 2.0f) { - collisions.clear(); - glm::vec3 neighborNormal = neighborSign * faceNormals[j]; - - // combine the face and neighbor normals to get the edge normal - glm::vec3 edgeNormal = glm::normalize(faceNormal + neighborNormal); - // Step the sphere along the edge in the direction of thirdNormal, starting at one corner and - // moving to the other. Test the penetration (invarient) and contact (changing) at each point. - float delta = cubeSide / (float)(numSteps - 1); - glm::vec3 startPosition = cubeCenter + (0.5f * cubeSide) * (faceNormal + neighborNormal - thirdNormal); - for (int m = 0; m < numSteps; ++m) { - sphereCenter = startPosition + ((float)m * delta) * thirdNormal + (sphereRadius + offset) * edgeNormal; - - CollisionInfo* collision = ShapeCollider::sphereVsAACubeHelper(sphereCenter, sphereRadius, - cubeCenter, cubeSide, collisions); - if (collision) { - QFAIL_WITH_MESSAGE("sphere should NOT collide with cube edge.\n\t\t" - << "edgeNormal = " << edgeNormal); - } - } - - } - } - } - } - - // corners - for (float firstSign = -1.0f; firstSign < 2.0f; firstSign += 2.0f) { - glm::vec3 firstNormal = firstSign * faceNormals[0]; - for (float secondSign = -1.0f; secondSign < 2.0f; secondSign += 2.0f) { - glm::vec3 secondNormal = secondSign * faceNormals[1]; - for (float thirdSign = -1.0f; thirdSign < 2.0f; thirdSign += 2.0f) { - collisions.clear(); - glm::vec3 thirdNormal = thirdSign * faceNormals[2]; - - // the cornerNormal is the normalized sum of the three faces - glm::vec3 cornerNormal = glm::normalize(firstNormal + secondNormal + thirdNormal); - - // compute a direction that is slightly offset from cornerNormal - glm::vec3 perpAxis = glm::normalize(glm::cross(cornerNormal, firstNormal)); - glm::vec3 nearbyAxis = glm::normalize(cornerNormal + 0.3f * perpAxis); - - // swing the sphere on a small cone that starts at the corner and is centered on the cornerNormal - float delta = TWO_PI / (float)(numSteps - 1); - for (int i = 0; i < numSteps; i++) { - float angle = (float)i * delta; - glm::quat rotation = glm::angleAxis(angle, cornerNormal); - glm::vec3 offsetAxis = rotation * nearbyAxis; - sphereCenter = cubeCenter + (SQUARE_ROOT_OF_3 * 0.5f * cubeSide) * cornerNormal + (sphereRadius + offset) * offsetAxis; - - CollisionInfo* collision = ShapeCollider::sphereVsAACubeHelper(sphereCenter, sphereRadius, - cubeCenter, cubeSide, collisions); - - if (collision) { - - QFAIL_WITH_MESSAGE("sphere should NOT collide with cube corner\n\t\t" << - "cornerNormal = " << cornerNormal); - break; - } - } - } - } - } -} - -void ShapeColliderTests::sphereTouchesAACubeFaces() { - CollisionList collisions(16); - - float sphereRadius = 1.13f; - glm::vec3 sphereCenter(0.0f); - - glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f); - float cubeSide = 4.34f; - - glm::vec3 faceNormals[] = {xAxis, yAxis, zAxis}; - int numDirections = 3; - - for (int i = 0; i < numDirections; ++i) { - // loop over both sides of cube positive and negative - for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) { - glm::vec3 faceNormal = sign * faceNormals[i]; - // outside - { - collisions.clear(); - float overlap = 0.25f * sphereRadius; - float parallelOffset = 0.5f * cubeSide + sphereRadius - overlap; - float perpOffset = 0.25f * cubeSide; - glm::vec3 expectedPenetration = - overlap * faceNormal; - - // We rotate the position of the sphereCenter about a circle on the cube face so that - // it hits the same face in multiple spots. The penetration should be invarient for - // all collisions. - float delta = TWO_PI / 4.0f; - for (float angle = 0; angle < TWO_PI + EPSILON; angle += delta) { - glm::quat rotation = glm::angleAxis(angle, faceNormal); - glm::vec3 perpAxis = rotation * faceNormals[(i + 1) % numDirections]; - - sphereCenter = cubeCenter + parallelOffset * faceNormal + perpOffset * perpAxis; - - CollisionInfo* collision = ShapeCollider::sphereVsAACubeHelper(sphereCenter, sphereRadius, - cubeCenter, cubeSide, collisions); - - if (!collision) { - - QFAIL_WITH_MESSAGE("sphere should collide outside cube face\n\t\t" << - "faceNormal = " << faceNormal); - break; - } - - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, EPSILON); - - glm::vec3 expectedContact = sphereCenter - sphereRadius * faceNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContact, EPSILON); - QCOMPARE(collision->getShapeA(), (Shape*)nullptr); - QCOMPARE(collision->getShapeB(), (Shape*)nullptr); - } - } - - // inside - { - collisions.clear(); - float overlap = 1.25f * sphereRadius; - float sphereOffset = 0.5f * cubeSide + sphereRadius - overlap; - sphereCenter = cubeCenter + sphereOffset * faceNormal; - - CollisionInfo* collision = ShapeCollider::sphereVsAACubeHelper(sphereCenter, sphereRadius, - cubeCenter, cubeSide, collisions); - - if (!collision) { - QFAIL_WITH_MESSAGE("sphere should collide inside cube face.\n\t\t" - << "faceNormal = " << faceNormal); - break; - } - - glm::vec3 expectedPenetration = - overlap * faceNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, EPSILON); - - glm::vec3 expectedContact = sphereCenter - sphereRadius * faceNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContact, EPSILON); - } - } - } -} - -void ShapeColliderTests::sphereTouchesAACubeEdges() { - CollisionList collisions(20); - - float sphereRadius = 1.37f; - glm::vec3 sphereCenter(0.0f); - - glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f); - float cubeSide = 2.98f; - - float overlap = 0.25f * sphereRadius; - int numSteps = 5; - - glm::vec3 faceNormals[] = {xAxis, yAxis, zAxis}; - int numDirections = 3; - - // loop over each face... - for (int i = 0; i < numDirections; ++i) { - for (float faceSign = -1.0f; faceSign < 2.0f; faceSign += 2.0f) { - glm::vec3 faceNormal = faceSign * faceNormals[i]; - - // loop over each neighboring face... - for (int j = (i + 1) % numDirections; j != i; j = (j + 1) % numDirections) { - // Compute the index to the third direction, which points perpendicular to both the face - // and the neighbor face. - int k = (j + 1) % numDirections; - if (k == i) { - k = (i + 1) % numDirections; - } - glm::vec3 thirdNormal = faceNormals[k]; - - for (float neighborSign = -1.0f; neighborSign < 2.0f; neighborSign += 2.0f) { - collisions.clear(); - glm::vec3 neighborNormal = neighborSign * faceNormals[j]; - - // combine the face and neighbor normals to get the edge normal - glm::vec3 edgeNormal = glm::normalize(faceNormal + neighborNormal); - - // Step the sphere along the edge in the direction of thirdNormal, starting at one corner and - // moving to the other. Test the penetration (invarient) and contact (changing) at each point. - glm::vec3 expectedPenetration = - overlap * edgeNormal; - float delta = cubeSide / (float)(numSteps - 1); - glm::vec3 startPosition = cubeCenter + (0.5f * cubeSide) * (faceNormal + neighborNormal - thirdNormal); - for (int m = 0; m < numSteps; ++m) { - sphereCenter = startPosition + ((float)m * delta) * thirdNormal + (sphereRadius - overlap) * edgeNormal; - - CollisionInfo* collision = ShapeCollider::sphereVsAACubeHelper(sphereCenter, sphereRadius, - cubeCenter, cubeSide, collisions); - if (!collision) { - QFAIL_WITH_MESSAGE("sphere should collide with cube edge.\n\t\t" - << "edgeNormal = " << edgeNormal); - break; - } - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, EPSILON); - - glm::vec3 expectedContact = sphereCenter - sphereRadius * edgeNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContact, EPSILON); - } - } - } - } - } -} - -void ShapeColliderTests::sphereTouchesAACubeCorners() { - CollisionList collisions(20); - - float sphereRadius = 1.37f; - glm::vec3 sphereCenter(0.0f); - - glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f); - float cubeSide = 2.98f; - - float overlap = 0.25f * sphereRadius; - int numSteps = 5; - - glm::vec3 faceNormals[] = {xAxis, yAxis, zAxis}; - - for (float firstSign = -1.0f; firstSign < 2.0f; firstSign += 2.0f) { - glm::vec3 firstNormal = firstSign * faceNormals[0]; - for (float secondSign = -1.0f; secondSign < 2.0f; secondSign += 2.0f) { - glm::vec3 secondNormal = secondSign * faceNormals[1]; - for (float thirdSign = -1.0f; thirdSign < 2.0f; thirdSign += 2.0f) { - collisions.clear(); - glm::vec3 thirdNormal = thirdSign * faceNormals[2]; - - // the cornerNormal is the normalized sum of the three faces - glm::vec3 cornerNormal = glm::normalize(firstNormal + secondNormal + thirdNormal); - - // compute a direction that is slightly offset from cornerNormal - glm::vec3 perpAxis = glm::normalize(glm::cross(cornerNormal, firstNormal)); - glm::vec3 nearbyAxis = glm::normalize(cornerNormal + 0.1f * perpAxis); - - // swing the sphere on a small cone that starts at the corner and is centered on the cornerNormal - float delta = TWO_PI / (float)(numSteps - 1); - for (int i = 0; i < numSteps; i++) { - float angle = (float)i * delta; - glm::quat rotation = glm::angleAxis(angle, cornerNormal); - glm::vec3 offsetAxis = rotation * nearbyAxis; - sphereCenter = cubeCenter + (SQUARE_ROOT_OF_3 * 0.5f * cubeSide) * cornerNormal + (sphereRadius - overlap) * offsetAxis; - - CollisionInfo* collision = ShapeCollider::sphereVsAACubeHelper(sphereCenter, sphereRadius, - cubeCenter, cubeSide, collisions); - - if (!collision) { - QFAIL_WITH_MESSAGE("sphere should collide with cube corner.\n\t\t" - << "cornerNormal = " << cornerNormal); - break; - } - - glm::vec3 expectedPenetration = - overlap * offsetAxis; - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, EPSILON); - - glm::vec3 expectedContact = sphereCenter - sphereRadius * offsetAxis; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContact, EPSILON); - } - } - } - } -} - -void ShapeColliderTests::capsuleMissesAACube() { - CollisionList collisions(16); - - float capsuleRadius = 1.0f; - - glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f); - float cubeSide = 2.0f; - AACubeShape cube(cubeSide, cubeCenter); - - glm::vec3 faceNormals[] = {xAxis, yAxis, zAxis}; - int numDirections = 3; - - float offset = 2.0f * EPSILON; - - // capsule caps miss cube faces - for (int i = 0; i < numDirections; ++i) { - for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) { - glm::vec3 faceNormal = sign * faceNormals[i]; - glm::vec3 secondNormal = faceNormals[(i + 1) % numDirections]; - glm::vec3 thirdNormal = faceNormals[(i + 2) % numDirections]; - - // pick a random point somewhere above the face - glm::vec3 startPoint = cubeCenter + (cubeSide + capsuleRadius) * faceNormal + - (cubeSide * (randFloat() - 0.5f)) * secondNormal + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // pick a second random point slightly more than one radius above the face - glm::vec3 endPoint = cubeCenter + (0.5f * cubeSide + capsuleRadius + offset) * faceNormal + - (cubeSide * (randFloat() - 0.5f)) * secondNormal + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // randomly swap the points so capsule axis may point toward or away from face - if (randFloat() > 0.5f) { - glm::vec3 temp = startPoint; - startPoint = endPoint; - endPoint = temp; - } - - // create a capsule between the points - CapsuleShape capsule(capsuleRadius, startPoint, endPoint); - - // collide capsule with cube - QCOMPARE(ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions), false); - } - } - - // capsule caps miss cube edges - // loop over each face... - for (int i = 0; i < numDirections; ++i) { - for (float faceSign = -1.0f; faceSign < 2.0f; faceSign += 2.0f) { - glm::vec3 faceNormal = faceSign * faceNormals[i]; - - // loop over each neighboring face... - for (int j = (i + 1) % numDirections; j != i; j = (j + 1) % numDirections) { - // Compute the index to the third direction, which points perpendicular to both the face - // and the neighbor face. - int k = (j + 1) % numDirections; - if (k == i) { - k = (i + 1) % numDirections; - } - glm::vec3 thirdNormal = faceNormals[k]; - - collisions.clear(); - for (float neighborSign = -1.0f; neighborSign < 2.0f; neighborSign += 2.0f) { - glm::vec3 neighborNormal = neighborSign * faceNormals[j]; - - // combine the face and neighbor normals to get the edge normal - glm::vec3 edgeNormal = glm::normalize(faceNormal + neighborNormal); - - // pick a random point somewhere above the edge - glm::vec3 startPoint = cubeCenter + (SQUARE_ROOT_OF_2 * cubeSide + capsuleRadius) * edgeNormal + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // pick a second random point slightly more than one radius above the edge - glm::vec3 endPoint = cubeCenter + (SQUARE_ROOT_OF_2 * 0.5f * cubeSide + capsuleRadius + offset) * edgeNormal + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // randomly swap the points so capsule axis may point toward or away from edge - if (randFloat() > 0.5f) { - glm::vec3 temp = startPoint; - startPoint = endPoint; - endPoint = temp; - } - - // create a capsule between the points - CapsuleShape capsule(capsuleRadius, startPoint, endPoint); - - // collide capsule with cube - QCOMPARE(ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions), false); - } - } - } - } - - // capsule caps miss cube corners - for (float firstSign = -1.0f; firstSign < 2.0f; firstSign += 2.0f) { - glm::vec3 firstNormal = firstSign * faceNormals[0]; - for (float secondSign = -1.0f; secondSign < 2.0f; secondSign += 2.0f) { - glm::vec3 secondNormal = secondSign * faceNormals[1]; - for (float thirdSign = -1.0f; thirdSign < 2.0f; thirdSign += 2.0f) { - collisions.clear(); - glm::vec3 thirdNormal = thirdSign * faceNormals[2]; - - // the cornerNormal is the normalized sum of the three faces - glm::vec3 cornerNormal = glm::normalize(firstNormal + secondNormal + thirdNormal); - - // pick a random point somewhere above the corner - glm::vec3 startPoint = cubeCenter + (SQUARE_ROOT_OF_3 * cubeSide + capsuleRadius) * cornerNormal + - (0.25f * cubeSide * (randFloat() - 0.5f)) * firstNormal + - (0.25f * cubeSide * (randFloat() - 0.5f)) * secondNormal + - (0.25f * cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // pick a second random point slightly more than one radius above the corner - glm::vec3 endPoint = cubeCenter + (SQUARE_ROOT_OF_3 * 0.5f * cubeSide + capsuleRadius + offset) * cornerNormal; - - // randomly swap the points so capsule axis may point toward or away from corner - if (randFloat() > 0.5f) { - glm::vec3 temp = startPoint; - startPoint = endPoint; - endPoint = temp; - } - - // create a capsule between the points - CapsuleShape capsule(capsuleRadius, startPoint, endPoint); - - // collide capsule with cube - QCOMPARE(ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions), false); - } - } - } - - // capsule sides almost hit cube edges - // loop over each face... - float capsuleLength = 2.0f; - for (int i = 0; i < numDirections; ++i) { - for (float faceSign = -1.0f; faceSign < 2.0f; faceSign += 2.0f) { - glm::vec3 faceNormal = faceSign * faceNormals[i]; - - // loop over each neighboring face... - for (int j = (i + 1) % numDirections; j != i; j = (j + 1) % numDirections) { - // Compute the index to the third direction, which points perpendicular to both the face - // and the neighbor face. - int k = (j + 1) % numDirections; - if (k == i) { - k = (i + 1) % numDirections; - } - glm::vec3 thirdNormal = faceNormals[k]; - - collisions.clear(); - for (float neighborSign = -1.0f; neighborSign < 2.0f; neighborSign += 2.0f) { - glm::vec3 neighborNormal = neighborSign * faceNormals[j]; - - // combine the face and neighbor normals to get the edge normal - glm::vec3 edgeNormal = glm::normalize(faceNormal + neighborNormal); - - // pick a random point somewhere along the edge - glm::vec3 edgePoint = cubeCenter + (SQUARE_ROOT_OF_2 * 0.5f * cubeSide) * edgeNormal + - ((cubeSide - 2.0f * offset) * (randFloat() - 0.5f)) * thirdNormal; - - // pick a random normal that is deflected slightly from edgeNormal - glm::vec3 deflectedNormal = glm::normalize(edgeNormal + - (0.1f * (randFloat() - 0.5f)) * faceNormal + - (0.1f * (randFloat() - 0.5f)) * neighborNormal); - - // compute the axis direction, which will be perp to deflectedNormal and thirdNormal - glm::vec3 axisDirection = glm::normalize(glm::cross(deflectedNormal, thirdNormal)); - - // compute a point for the capsule's axis along deflection normal away from edgePoint - glm::vec3 axisPoint = edgePoint + (capsuleRadius + offset) * deflectedNormal; - - // now we can compute the capsule endpoints - glm::vec3 endPoint = axisPoint + (0.5f * capsuleLength * randFloat()) * axisDirection; - glm::vec3 startPoint = axisPoint - (0.5f * capsuleLength * randFloat()) * axisDirection; - - // create a capsule between the points - CapsuleShape capsule(capsuleRadius, startPoint, endPoint); - - // collide capsule with cube - QCOMPARE(ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions), false); - } - } - } - } - - // capsule sides almost hit cube corners - for (float firstSign = -1.0f; firstSign < 2.0f; firstSign += 2.0f) { - glm::vec3 firstNormal = firstSign * faceNormals[0]; - for (float secondSign = -1.0f; secondSign < 2.0f; secondSign += 2.0f) { - glm::vec3 secondNormal = secondSign * faceNormals[1]; - for (float thirdSign = -1.0f; thirdSign < 2.0f; thirdSign += 2.0f) { - collisions.clear(); - glm::vec3 thirdNormal = thirdSign * faceNormals[2]; - - // the cornerNormal is the normalized sum of the three faces - glm::vec3 cornerNormal = glm::normalize(firstNormal + secondNormal + thirdNormal); - - // compute a penetration normal that is somewhat randomized about cornerNormal - glm::vec3 penetrationNormal = - glm::normalize(cornerNormal + - (0.05f * cubeSide * (randFloat() - 0.5f)) * firstNormal + - (0.05f * cubeSide * (randFloat() - 0.5f)) * secondNormal + - (0.05f * cubeSide * (randFloat() - 0.5f)) * thirdNormal); - - // pick a random point somewhere above the corner - glm::vec3 corner = cubeCenter + (0.5f * cubeSide) * (firstNormal + secondNormal + thirdNormal); - glm::vec3 startPoint = corner + (3.0f * cubeSide) * cornerNormal + - (0.25f * cubeSide * (randFloat() - 0.5f)) * firstNormal + - (0.25f * cubeSide * (randFloat() - 0.5f)) * secondNormal + - (0.25f * cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // pick a second random point slightly less than one radius above the corner - // with some sight perp motion - glm::vec3 endPoint = corner - (capsuleRadius + offset) * penetrationNormal; - - // randomly swap the points so capsule axis may point toward or away from corner - if (randFloat() > 0.5f) { - glm::vec3 temp = startPoint; - startPoint = endPoint; - endPoint = temp; - } - - // create a capsule between the points - CapsuleShape capsule(capsuleRadius, startPoint, endPoint); - - // collide capsule with cube - QCOMPARE(ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions), false); - } - } - } - - // capsule sides almost hit cube faces - // these are the steps along the capsuleAxis where we'll put the capsule endpoints - float steps[] = { -1.0f, 2.0f, 0.25f, 0.75f, -1.0f }; - - for (int i = 0; i < numDirections; ++i) { - for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) { - glm::vec3 faceNormal = sign * faceNormals[i]; - glm::vec3 secondNormal = faceNormals[(i + 1) % numDirections]; - glm::vec3 thirdNormal = faceNormals[(i + 2) % numDirections]; - - // pick two random point on opposite edges of the face - glm::vec3 firstEdgeIntersection = cubeCenter + (0.5f * cubeSide) * (faceNormal + secondNormal) + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - glm::vec3 secondEdgeIntersection = cubeCenter + (0.5f * cubeSide) * (faceNormal - secondNormal) + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // compute the un-normalized axis for the capsule - glm::vec3 capsuleAxis = secondEdgeIntersection - firstEdgeIntersection; - // there are three pairs in steps[] - for (int j = 0; j < 4; j++) { - collisions.clear(); - glm::vec3 startPoint = firstEdgeIntersection + steps[j] * capsuleAxis + (capsuleRadius + offset) * faceNormal; - glm::vec3 endPoint = firstEdgeIntersection + steps[j + 1] * capsuleAxis + (capsuleRadius + offset) * faceNormal; - - // create a capsule between the points - CapsuleShape capsule(capsuleRadius, startPoint, endPoint); - - // collide capsule with cube - QCOMPARE(ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions), false); - } - } - } -} - -void ShapeColliderTests::capsuleTouchesAACube() { - CollisionList collisions(16); - - float capsuleRadius = 1.0f; - - glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f); - float cubeSide = 2.0f; - AACubeShape cube(cubeSide, cubeCenter); - - glm::vec3 faceNormals[] = {xAxis, yAxis, zAxis}; - int numDirections = 3; - - float overlap = 0.25f * capsuleRadius; - float allowableError = 10.0f * EPSILON; - - // capsule caps hit cube faces - for (int i = 0; i < numDirections; ++i) { - for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) { - glm::vec3 faceNormal = sign * faceNormals[i]; - glm::vec3 secondNormal = faceNormals[(i + 1) % numDirections]; - glm::vec3 thirdNormal = faceNormals[(i + 2) % numDirections]; - - // pick a random point somewhere above the face - glm::vec3 startPoint = cubeCenter + (cubeSide + capsuleRadius) * faceNormal + - (cubeSide * (randFloat() - 0.5f)) * secondNormal + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // pick a second random point slightly less than one radius above the face - // (but reduce width of range by 2*overlap to prevent the penetration from - // registering against other faces) - glm::vec3 endPoint = cubeCenter + (0.5f * cubeSide + capsuleRadius - overlap) * faceNormal + - ((cubeSide - 2.0f * overlap) * (randFloat() - 0.5f)) * secondNormal + - ((cubeSide - 2.0f * overlap) * (randFloat() - 0.5f)) * thirdNormal; - glm::vec3 collidingPoint = endPoint; - - // randomly swap the points so capsule axis may point toward or away from face - if (randFloat() > 0.5f) { - glm::vec3 temp = startPoint; - startPoint = endPoint; - endPoint = temp; - } - - // create a capsule between the points - CapsuleShape capsule(capsuleRadius, startPoint, endPoint); - - // collide capsule with cube - QCOMPARE(ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions), true); - - CollisionInfo* collision = collisions.getLastCollision(); - - // penetration points from capsule into cube - glm::vec3 expectedPenetration = - overlap * faceNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, allowableError); - - // contactPoint is on surface of capsule - glm::vec3 expectedContactPoint = collidingPoint - capsuleRadius * faceNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, allowableError); - } - } - - // capsule caps hit cube edges - // loop over each face... - for (int i = 0; i < numDirections; ++i) { - for (float faceSign = -1.0f; faceSign < 2.0f; faceSign += 2.0f) { - glm::vec3 faceNormal = faceSign * faceNormals[i]; - - // loop over each neighboring face... - for (int j = (i + 1) % numDirections; j != i; j = (j + 1) % numDirections) { - // Compute the index to the third direction, which points perpendicular to both the face - // and the neighbor face. - int k = (j + 1) % numDirections; - if (k == i) { - k = (i + 1) % numDirections; - } - glm::vec3 thirdNormal = faceNormals[k]; - - collisions.clear(); - for (float neighborSign = -1.0f; neighborSign < 2.0f; neighborSign += 2.0f) { - glm::vec3 neighborNormal = neighborSign * faceNormals[j]; - - // combine the face and neighbor normals to get the edge normal - glm::vec3 edgeNormal = glm::normalize(faceNormal + neighborNormal); - - // pick a random point somewhere above the edge - glm::vec3 startPoint = cubeCenter + (SQUARE_ROOT_OF_2 * cubeSide + capsuleRadius) * edgeNormal + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // pick a second random point slightly less than one radius above the edge - glm::vec3 endPoint = cubeCenter + (SQUARE_ROOT_OF_2 * 0.5f * cubeSide + capsuleRadius - overlap) * edgeNormal + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - glm::vec3 collidingPoint = endPoint; - - // randomly swap the points so capsule axis may point toward or away from edge - if (randFloat() > 0.5f) { - glm::vec3 temp = startPoint; - startPoint = endPoint; - endPoint = temp; - } - - // create a capsule between the points - CapsuleShape capsule(capsuleRadius, startPoint, endPoint); - - // collide capsule with cube - QCOMPARE(ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions), true); - - CollisionInfo* collision = collisions.getLastCollision(); - QCOMPARE(collision != nullptr, true); - - // penetration points from capsule into cube - glm::vec3 expectedPenetration = - overlap * edgeNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, allowableError); - - // contactPoint is on surface of capsule - glm::vec3 expectedContactPoint = collidingPoint - capsuleRadius * edgeNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, allowableError); - } - } - } - } - - // capsule caps hit cube corners - for (float firstSign = -1.0f; firstSign < 2.0f; firstSign += 2.0f) { - glm::vec3 firstNormal = firstSign * faceNormals[0]; - for (float secondSign = -1.0f; secondSign < 2.0f; secondSign += 2.0f) { - glm::vec3 secondNormal = secondSign * faceNormals[1]; - for (float thirdSign = -1.0f; thirdSign < 2.0f; thirdSign += 2.0f) { - collisions.clear(); - glm::vec3 thirdNormal = thirdSign * faceNormals[2]; - - // the cornerNormal is the normalized sum of the three faces - glm::vec3 cornerNormal = glm::normalize(firstNormal + secondNormal + thirdNormal); - - // pick a random point somewhere above the corner - glm::vec3 startPoint = cubeCenter + (SQUARE_ROOT_OF_3 * cubeSide + capsuleRadius) * cornerNormal + - (0.25f * cubeSide * (randFloat() - 0.5f)) * firstNormal + - (0.25f * cubeSide * (randFloat() - 0.5f)) * secondNormal + - (0.25f * cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // pick a second random point slightly less than one radius above the corner - glm::vec3 endPoint = cubeCenter + (SQUARE_ROOT_OF_3 * 0.5f * cubeSide + capsuleRadius - overlap) * cornerNormal; - glm::vec3 collidingPoint = endPoint; - - // randomly swap the points so capsule axis may point toward or away from corner - if (randFloat() > 0.5f) { - glm::vec3 temp = startPoint; - startPoint = endPoint; - endPoint = temp; - } - - // create a capsule between the points - CapsuleShape capsule(capsuleRadius, startPoint, endPoint); - - // collide capsule with cube - QCOMPARE(ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions), true); - - CollisionInfo* collision = collisions.getLastCollision(); - QCOMPARE(collision != nullptr, true); - - // penetration points from capsule into cube - glm::vec3 expectedPenetration = - overlap * cornerNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, allowableError); - - // contactPoint is on surface of capsule - glm::vec3 expectedContactPoint = collidingPoint - capsuleRadius * cornerNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, allowableError); - } - } - } - - // capsule sides hit cube edges - // loop over each face... - float capsuleLength = 2.0f; - for (int i = 0; i < numDirections; ++i) { - for (float faceSign = -1.0f; faceSign < 2.0f; faceSign += 2.0f) { - glm::vec3 faceNormal = faceSign * faceNormals[i]; - - // loop over each neighboring face... - for (int j = (i + 1) % numDirections; j != i; j = (j + 1) % numDirections) { - // Compute the index to the third direction, which points perpendicular to both the face - // and the neighbor face. - int k = (j + 1) % numDirections; - if (k == i) { - k = (i + 1) % numDirections; - } - glm::vec3 thirdNormal = faceNormals[k]; - - collisions.clear(); - for (float neighborSign = -1.0f; neighborSign < 2.0f; neighborSign += 2.0f) { - glm::vec3 neighborNormal = neighborSign * faceNormals[j]; - - // combine the face and neighbor normals to get the edge normal - glm::vec3 edgeNormal = glm::normalize(faceNormal + neighborNormal); - - // pick a random point somewhere along the edge - glm::vec3 edgePoint = cubeCenter + (SQUARE_ROOT_OF_2 * 0.5f * cubeSide) * edgeNormal + - ((cubeSide - 2.0f * overlap) * (randFloat() - 0.5f)) * thirdNormal; - - // pick a random normal that is deflected slightly from edgeNormal - glm::vec3 deflectedNormal = glm::normalize(edgeNormal + - (0.1f * (randFloat() - 0.5f)) * faceNormal + - (0.1f * (randFloat() - 0.5f)) * neighborNormal); - - // compute the axis direction, which will be perp to deflectedNormal and thirdNormal - glm::vec3 axisDirection = glm::normalize(glm::cross(deflectedNormal, thirdNormal)); - - // compute a point for the capsule's axis along deflection normal away from edgePoint - glm::vec3 axisPoint = edgePoint + (capsuleRadius - overlap) * deflectedNormal; - - // now we can compute the capsule endpoints - glm::vec3 endPoint = axisPoint + (0.5f * capsuleLength * randFloat()) * axisDirection; - glm::vec3 startPoint = axisPoint - (0.5f * capsuleLength * randFloat()) * axisDirection; - - // create a capsule between the points - CapsuleShape capsule(capsuleRadius, startPoint, endPoint); - - // collide capsule with cube - QCOMPARE(ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions), true); - - CollisionInfo* collision = collisions.getLastCollision(); - QCOMPARE(collision != nullptr, true); - - // penetration points from capsule into cube - glm::vec3 expectedPenetration = - overlap * deflectedNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, allowableError); - - // contactPoint is on surface of capsule - glm::vec3 expectedContactPoint = axisPoint - capsuleRadius * deflectedNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, allowableError); } - } - } - } - - // capsule sides hit cube corners - for (float firstSign = -1.0f; firstSign < 2.0f; firstSign += 2.0f) { - glm::vec3 firstNormal = firstSign * faceNormals[0]; - for (float secondSign = -1.0f; secondSign < 2.0f; secondSign += 2.0f) { - glm::vec3 secondNormal = secondSign * faceNormals[1]; - for (float thirdSign = -1.0f; thirdSign < 2.0f; thirdSign += 2.0f) { - collisions.clear(); - glm::vec3 thirdNormal = thirdSign * faceNormals[2]; - - // the cornerNormal is the normalized sum of the three faces - glm::vec3 cornerNormal = glm::normalize(firstNormal + secondNormal + thirdNormal); - - // compute a penetration normal that is somewhat randomized about cornerNormal - glm::vec3 penetrationNormal = - glm::normalize(cornerNormal + - (0.05f * cubeSide * (randFloat() - 0.5f)) * firstNormal + - (0.05f * cubeSide * (randFloat() - 0.5f)) * secondNormal + - (0.05f * cubeSide * (randFloat() - 0.5f)) * thirdNormal); - - // pick a random point somewhere above the corner - glm::vec3 corner = cubeCenter + (0.5f * cubeSide) * (firstNormal + secondNormal + thirdNormal); - glm::vec3 startPoint = corner + (3.0f * cubeSide) * cornerNormal + - (0.25f * cubeSide * (randFloat() - 0.5f)) * firstNormal + - (0.25f * cubeSide * (randFloat() - 0.5f)) * secondNormal + - (0.25f * cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // pick a second random point slightly less than one radius above the corner - // with some sight perp motion - glm::vec3 endPoint = corner - (capsuleRadius - overlap) * penetrationNormal; - glm::vec3 collidingPoint = endPoint; - - // randomly swap the points so capsule axis may point toward or away from corner - if (randFloat() > 0.5f) { - glm::vec3 temp = startPoint; - startPoint = endPoint; - endPoint = temp; - } - - // create a capsule between the points - CapsuleShape capsule(capsuleRadius, startPoint, endPoint); - - // collide capsule with cube - QCOMPARE(ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions), true); - - CollisionInfo* collision = collisions.getLastCollision(); - QCOMPARE(collision != nullptr, true); - - // penetration points from capsule into cube - glm::vec3 expectedPenetration = overlap * penetrationNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, allowableError); - - // contactPoint is on surface of capsule - glm::vec3 expectedContactPoint = collidingPoint + capsuleRadius * penetrationNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, allowableError); - } - } - } - - // capsule sides hit cube faces - // these are the steps along the capsuleAxis where we'll put the capsule endpoints - float steps[] = { -1.0f, 2.0f, 0.25f, 0.75f, -1.0f }; - - for (int i = 0; i < numDirections; ++i) { - for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) { - glm::vec3 faceNormal = sign * faceNormals[i]; - glm::vec3 secondNormal = faceNormals[(i + 1) % numDirections]; - glm::vec3 thirdNormal = faceNormals[(i + 2) % numDirections]; - - // pick two random point on opposite edges of the face - glm::vec3 firstEdgeIntersection = cubeCenter + (0.5f * cubeSide) * (faceNormal + secondNormal) + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - glm::vec3 secondEdgeIntersection = cubeCenter + (0.5f * cubeSide) * (faceNormal - secondNormal) + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // compute the un-normalized axis for the capsule - glm::vec3 capsuleAxis = secondEdgeIntersection - firstEdgeIntersection; - // there are three pairs in steps[] - for (int j = 0; j < 4; j++) { - collisions.clear(); - glm::vec3 startPoint = firstEdgeIntersection + steps[j] * capsuleAxis + (capsuleRadius - overlap) * faceNormal; - glm::vec3 endPoint = firstEdgeIntersection + steps[j + 1] * capsuleAxis + (capsuleRadius - overlap) * faceNormal; - - // create a capsule between the points - CapsuleShape capsule(capsuleRadius, startPoint, endPoint); - - // collide capsule with cube - QCOMPARE(ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions), true); - QCOMPARE(collisions.size(), 2); - - // compute the expected contact points - // NOTE: whether the startPoint or endPoint are expected to collide depends the relative values - // of the steps[] that were used to compute them above. - glm::vec3 expectedContactPoints[2]; - if (j == 0) { - expectedContactPoints[0] = firstEdgeIntersection - overlap * faceNormal; - expectedContactPoints[1] = secondEdgeIntersection - overlap * faceNormal; - } else if (j == 1) { - expectedContactPoints[0] = secondEdgeIntersection - overlap * faceNormal; - expectedContactPoints[1] = endPoint - capsuleRadius * faceNormal; - } else if (j == 2) { - expectedContactPoints[0] = startPoint - capsuleRadius * faceNormal; - expectedContactPoints[1] = endPoint - capsuleRadius * faceNormal; - } else if (j == 3) { - expectedContactPoints[0] = startPoint - capsuleRadius * faceNormal; - expectedContactPoints[1] = firstEdgeIntersection - overlap * faceNormal; - } - - // verify each contact - for (int k = 0; k < 2; ++k) { - CollisionInfo* collision = collisions.getCollision(k); - // penetration points from capsule into cube - glm::vec3 expectedPenetration = - overlap * faceNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, allowableError); - - // the order of the final contact points is undefined, so we - // figure out which expected contact point is the closest to the real one - // and then verify accuracy on that - float length0 = glm::length(collision->_contactPoint - expectedContactPoints[0]); - float length1 = glm::length(collision->_contactPoint - expectedContactPoints[1]); - glm::vec3 expectedContactPoint = (length0 < length1) ? expectedContactPoints[0] : expectedContactPoints[1]; - // contactPoint is on surface of capsule - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, allowableError); - } - } - } - } -} - - -void ShapeColliderTests::rayHitsSphere() { - float startDistance = 3.0f; - - float radius = 1.0f; - glm::vec3 center(0.0f); - SphereShape sphere(radius, center); - - // very simple ray along xAxis - { - RayIntersectionInfo intersection; - intersection._rayStart = -startDistance * xAxis; - intersection._rayDirection = xAxis; - - QCOMPARE(sphere.findRayIntersection(intersection), true); - - float expectedDistance = startDistance - radius; - QCOMPARE_WITH_ABS_ERROR(intersection._hitDistance, expectedDistance, startDistance * EPSILON); - QCOMPARE(intersection._hitShape, &sphere); - } - - // ray along a diagonal axis - { - RayIntersectionInfo intersection; - intersection._rayStart = glm::vec3(startDistance, startDistance, 0.0f); - intersection._rayDirection = - glm::normalize(intersection._rayStart); - QCOMPARE(sphere.findRayIntersection(intersection), true); - - float expectedDistance = SQUARE_ROOT_OF_2 * startDistance - radius; - QCOMPARE_WITH_ABS_ERROR(intersection._hitDistance, expectedDistance, startDistance * EPSILON); - } - - // rotated and displaced ray and sphere - { - startDistance = 7.41f; - radius = 3.917f; - - glm::vec3 axis = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f)); - glm::quat rotation = glm::angleAxis(0.987654321f, axis); - glm::vec3 translation(35.7f, 2.46f, -1.97f); - - glm::vec3 unrotatedRayDirection = -xAxis; - glm::vec3 untransformedRayStart = startDistance * xAxis; - - RayIntersectionInfo intersection; - intersection._rayStart = rotation * (untransformedRayStart + translation); - intersection._rayDirection = rotation * unrotatedRayDirection; - - sphere.setRadius(radius); - sphere.setTranslation(rotation * translation); - - QCOMPARE(sphere.findRayIntersection(intersection), true); - float expectedDistance = startDistance - radius; - QCOMPARE_WITH_ABS_ERROR(intersection._hitDistance, expectedDistance, startDistance * EPSILON); - } -} - -void ShapeColliderTests::rayBarelyHitsSphere() { - float radius = 1.0f; - glm::vec3 center(0.0f); - float delta = 2.0f * EPSILON; - - SphereShape sphere(radius, center); - float startDistance = 3.0f; - - { - RayIntersectionInfo intersection; - intersection._rayStart = glm::vec3(-startDistance, radius - delta, 0.0f); - intersection._rayDirection = xAxis; - - // very simple ray along xAxis - QCOMPARE(sphere.findRayIntersection(intersection), true); - QCOMPARE(intersection._hitShape, &sphere); - } - - { - // translate and rotate the whole system... - glm::vec3 axis = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f)); - glm::quat rotation = glm::angleAxis(0.987654321f, axis); - glm::vec3 translation(35.7f, 0.46f, -1.97f); - - RayIntersectionInfo intersection; - intersection._rayStart = rotation * (intersection._rayStart + translation); - intersection._rayDirection = rotation * intersection._rayDirection; - - sphere.setTranslation(rotation * translation); - - // ...and test again - QCOMPARE(sphere.findRayIntersection(intersection), true); - } -} - - -void ShapeColliderTests::rayBarelyMissesSphere() { - // same as the barely-hits case, but this time we move the ray away from sphere - float radius = 1.0f; - glm::vec3 center(0.0f); - float delta = 2.0f * EPSILON; - - SphereShape sphere(radius, center); - float startDistance = 3.0f; - - { - RayIntersectionInfo intersection; - intersection._rayStart = glm::vec3(-startDistance, radius + delta, 0.0f); - intersection._rayDirection = xAxis; - - // very simple ray along xAxis - QCOMPARE(sphere.findRayIntersection(intersection), false); - QCOMPARE(intersection._hitDistance, FLT_MAX); - } - - { - // translate and rotate the whole system... - float angle = 0.987654321f; - glm::vec3 axis = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f)); - glm::quat rotation = glm::angleAxis(angle, axis); - glm::vec3 translation(35.7f, 2.46f, -1.97f); - - RayIntersectionInfo intersection; - intersection._rayStart = rotation * (glm::vec3(-startDistance, radius + delta, 0.0f) + translation); - intersection._rayDirection = rotation * xAxis; - sphere.setTranslation(rotation * translation); - - // ...and test again - QCOMPARE(sphere.findRayIntersection(intersection), false); - QCOMPARE(intersection._hitDistance == FLT_MAX, true); - QCOMPARE(intersection._hitShape == nullptr, true); - } -} - -void ShapeColliderTests::rayHitsCapsule() { - float startDistance = 3.0f; - float radius = 1.0f; - float halfHeight = 2.0f; - glm::vec3 center(0.0f); - CapsuleShape capsule(radius, halfHeight); - - // simple tests along xAxis - { // toward capsule center - RayIntersectionInfo intersection; - intersection._rayStart = glm::vec3(startDistance, 0.0f, 0.0f); - intersection._rayDirection = - xAxis; - QCOMPARE(capsule.findRayIntersection(intersection), true); - float expectedDistance = startDistance - radius; - QCOMPARE_WITH_ABS_ERROR(intersection._hitDistance, expectedDistance, startDistance * EPSILON); - QCOMPARE(intersection._hitShape, &capsule); - } - - { // toward top of cylindrical wall - RayIntersectionInfo intersection; - intersection._rayStart = glm::vec3(startDistance, halfHeight, 0.0f); - intersection._rayDirection = - xAxis; - QCOMPARE(capsule.findRayIntersection(intersection), true); - float expectedDistance = startDistance - radius; - QCOMPARE_WITH_ABS_ERROR(intersection._hitDistance, expectedDistance, startDistance * EPSILON); - } - - float delta = 2.0f * EPSILON; - { // toward top cap - RayIntersectionInfo intersection; - intersection._rayStart = glm::vec3(startDistance, halfHeight + delta, 0.0f); - intersection._rayDirection = - xAxis; - QCOMPARE(capsule.findRayIntersection(intersection), true); - float expectedDistance = startDistance - radius; - QCOMPARE_WITH_ABS_ERROR(intersection._hitDistance, expectedDistance, startDistance * EPSILON); - } - - const float EDGE_CASE_SLOP_FACTOR = 20.0f; - { // toward tip of top cap - RayIntersectionInfo intersection; - intersection._rayStart = glm::vec3(startDistance, halfHeight + radius - delta, 0.0f); - intersection._rayDirection = - xAxis; - QCOMPARE(capsule.findRayIntersection(intersection), true); - float expectedDistance = startDistance - radius * sqrtf(2.0f * delta); // using small angle approximation of cosine - QCOMPARE_WITH_ABS_ERROR(intersection._hitDistance, expectedDistance, startDistance * EDGE_CASE_SLOP_FACTOR * EPSILON); - } - - { // toward tip of bottom cap - RayIntersectionInfo intersection; - intersection._rayStart = glm::vec3(startDistance, - halfHeight - radius + delta, 0.0f); - intersection._rayDirection = - xAxis; - QCOMPARE(capsule.findRayIntersection(intersection), true); - float expectedDistance = startDistance - radius * sqrtf(2.0f * delta); // using small angle approximation of cosine - QCOMPARE_WITH_ABS_ERROR(intersection._hitDistance, expectedDistance, startDistance * EPSILON * EDGE_CASE_SLOP_FACTOR); - } - - { // toward edge of capsule cylindrical face - RayIntersectionInfo intersection; - intersection._rayStart = glm::vec3(startDistance, 0.0f, radius - delta); - intersection._rayDirection = - xAxis; - QCOMPARE(capsule.findRayIntersection(intersection), true); - float expectedDistance = startDistance - radius * sqrtf(2.0f * delta); // using small angle approximation of cosine - QCOMPARE_WITH_ABS_ERROR(intersection._hitDistance, expectedDistance, startDistance * EPSILON * EDGE_CASE_SLOP_FACTOR); - } - // TODO: test at steep angles near cylinder/cap junction -} - -void ShapeColliderTests::rayMissesCapsule() { - // same as edge case hit tests, but shifted in the opposite direction - float startDistance = 3.0f; - float radius = 1.0f; - float halfHeight = 2.0f; - glm::vec3 center(0.0f); - CapsuleShape capsule(radius, halfHeight); - - { // simple test along xAxis - // toward capsule center - RayIntersectionInfo intersection; - intersection._rayStart = glm::vec3(startDistance, 0.0f, 0.0f); - intersection._rayDirection = -xAxis; - float delta = 2.0f * EPSILON; - - // over top cap - intersection._rayStart.y = halfHeight + radius + delta; - intersection._hitDistance = FLT_MAX; - QCOMPARE(capsule.findRayIntersection(intersection), false); - QCOMPARE(intersection._hitDistance, FLT_MAX); - - // below bottom cap - intersection._rayStart.y = - halfHeight - radius - delta; - intersection._hitDistance = FLT_MAX; - QCOMPARE(capsule.findRayIntersection(intersection), false); - QCOMPARE(intersection._hitDistance, FLT_MAX); - - // past edge of capsule cylindrical face - intersection._rayStart.y = 0.0f; - intersection._rayStart.z = radius + delta; - intersection._hitDistance = FLT_MAX; - QCOMPARE(capsule.findRayIntersection(intersection), false); - QCOMPARE(intersection._hitDistance, FLT_MAX); - QCOMPARE(intersection._hitShape, (Shape*)nullptr); - } - // TODO: test at steep angles near edge -} - -void ShapeColliderTests::rayHitsPlane() { - // make a simple plane - float planeDistanceFromOrigin = 3.579f; - glm::vec3 planePosition(0.0f, planeDistanceFromOrigin, 0.0f); - PlaneShape plane; - plane.setPoint(planePosition); - plane.setNormal(yAxis); - - // make a simple ray - float startDistance = 1.234f; - { - RayIntersectionInfo intersection; - intersection._rayStart = -startDistance * xAxis; - intersection._rayDirection = glm::normalize(glm::vec3(1.0f, 1.0f, 1.0f)); - - QCOMPARE(plane.findRayIntersection(intersection), true); - float expectedDistance = SQUARE_ROOT_OF_3 * planeDistanceFromOrigin; - - QCOMPARE_WITH_ABS_ERROR(intersection._hitDistance, expectedDistance, planeDistanceFromOrigin * EPSILON); - QCOMPARE(intersection._hitShape, &plane); - } - - { // rotate the whole system and try again - float angle = 37.8f; - glm::vec3 axis = glm::normalize( glm::vec3(-7.0f, 2.8f, 9.3f) ); - glm::quat rotation = glm::angleAxis(angle, axis); - - plane.setNormal(rotation * yAxis); - plane.setPoint(rotation * planePosition); - RayIntersectionInfo intersection; - intersection._rayStart = rotation * (-startDistance * xAxis); - intersection._rayDirection = rotation * glm::normalize(glm::vec3(1.0f, 1.0f, 1.0f)); - - QCOMPARE(plane.findRayIntersection(intersection), true); - - float expectedDistance = SQUARE_ROOT_OF_3 * planeDistanceFromOrigin; - QCOMPARE_WITH_ABS_ERROR(intersection._hitDistance, expectedDistance, planeDistanceFromOrigin * EPSILON); - } -} - -void ShapeColliderTests::rayMissesPlane() { - // make a simple plane - float planeDistanceFromOrigin = 3.579f; - glm::vec3 planePosition(0.0f, planeDistanceFromOrigin, 0.0f); - PlaneShape plane; - plane.setTranslation(planePosition); - - { // parallel rays should miss - float startDistance = 1.234f; - RayIntersectionInfo intersection; - intersection._rayStart = glm::vec3(-startDistance, 0.0f, 0.0f); - intersection._rayDirection = glm::normalize(glm::vec3(-1.0f, 0.0f, -1.0f)); - - QCOMPARE(plane.findRayIntersection(intersection), false); - QCOMPARE(intersection._hitDistance, FLT_MAX); - - // rotate the whole system and try again - float angle = 37.8f; - glm::vec3 axis = glm::normalize( glm::vec3(-7.0f, 2.8f, 9.3f) ); - glm::quat rotation = glm::angleAxis(angle, axis); - - plane.setTranslation(rotation * planePosition); - plane.setRotation(rotation); - - intersection._rayStart = rotation * intersection._rayStart; - intersection._rayDirection = rotation * intersection._rayDirection; - intersection._hitDistance = FLT_MAX; - - QCOMPARE(plane.findRayIntersection(intersection), false); - QCOMPARE(intersection._hitDistance, FLT_MAX); - QCOMPARE(intersection._hitShape, (Shape*)nullptr); - } - - { // make a simple ray that points away from plane - float startDistance = 1.234f; - - RayIntersectionInfo intersection; - intersection._rayStart = glm::vec3(-startDistance, 0.0f, 0.0f); - intersection._rayDirection = glm::normalize(glm::vec3(-1.0f, -1.0f, -1.0f)); - intersection._hitDistance = FLT_MAX; - - QCOMPARE(plane.findRayIntersection(intersection), false); - QCOMPARE(intersection._hitDistance, FLT_MAX); - - // rotate the whole system and try again - float angle = 37.8f; - glm::vec3 axis = glm::normalize( glm::vec3(-7.0f, 2.8f, 9.3f) ); - glm::quat rotation = glm::angleAxis(angle, axis); - - plane.setTranslation(rotation * planePosition); - plane.setRotation(rotation); - - intersection._rayStart = rotation * intersection._rayStart; - intersection._rayDirection = rotation * intersection._rayDirection; - intersection._hitDistance = FLT_MAX; - - QCOMPARE(plane.findRayIntersection(intersection), false); - QCOMPARE(intersection._hitDistance, FLT_MAX); - } -} - -void ShapeColliderTests::rayHitsAACube() { - glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f); - float cubeSide = 2.127f; - AACubeShape cube(cubeSide, cubeCenter); - - float rayOffset = 3.796f; - - glm::vec3 faceNormals[] = {xAxis, yAxis, zAxis}; - int numDirections = 3; - int numRayCasts = 5; - - for (int i = 0; i < numDirections; ++i) { - for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) { - glm::vec3 faceNormal = sign * faceNormals[i]; - glm::vec3 secondNormal = faceNormals[(i + 1) % numDirections]; - glm::vec3 thirdNormal = faceNormals[(i + 2) % numDirections]; - - // pick a random point somewhere above the face - glm::vec3 rayStart = cubeCenter + - (cubeSide + rayOffset) * faceNormal + - (cubeSide * (randFloat() - 0.5f)) * secondNormal + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // cast multiple rays toward the face - for (int j = 0; j < numRayCasts; ++j) { - // pick a random point on the face - glm::vec3 facePoint = cubeCenter + - 0.5f * cubeSide * faceNormal + - (cubeSide * (randFloat() - 0.5f)) * secondNormal + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // construct a ray from first point through second point - RayIntersectionInfo intersection; - intersection._rayStart = rayStart; - intersection._rayDirection = glm::normalize(facePoint - rayStart); - intersection._rayLength = 1.0001f * glm::distance(rayStart, facePoint); - - // cast the ray -// bool hit = cube.findRayIntersection(intersection); - - // validate - QCOMPARE(cube.findRayIntersection(intersection), true); - QCOMPARE_WITH_ABS_ERROR(glm::dot(faceNormal, intersection._hitNormal), 1.0f, EPSILON); - QCOMPARE_WITH_ABS_ERROR(facePoint, intersection.getIntersectionPoint(), EPSILON); - QCOMPARE(intersection._hitShape, &cube); - } - } - } -} - -void ShapeColliderTests::rayMissesAACube() { - //glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f); - //float cubeSide = 2.127f; - glm::vec3 cubeCenter(0.0f); - float cubeSide = 2.0f; - AACubeShape cube(cubeSide, cubeCenter); - - float rayOffset = 3.796f; - - glm::vec3 faceNormals[] = {xAxis, yAxis, zAxis}; - int numDirections = 3; - int numRayCasts = 5; - - const float SOME_SMALL_NUMBER = 0.0001f; - - { // ray misses cube for being too short - for (int i = 0; i < numDirections; ++i) { - for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) { - glm::vec3 faceNormal = sign * faceNormals[i]; - glm::vec3 secondNormal = faceNormals[(i + 1) % numDirections]; - glm::vec3 thirdNormal = faceNormals[(i + 2) % numDirections]; - - // pick a random point somewhere above the face - glm::vec3 rayStart = cubeCenter + - (cubeSide + rayOffset) * faceNormal + - (cubeSide * (randFloat() - 0.5f)) * secondNormal + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // cast multiple rays toward the face - for (int j = 0; j < numRayCasts; ++j) { - // pick a random point on the face - glm::vec3 facePoint = cubeCenter + - 0.5f * cubeSide * faceNormal + - (cubeSide * (randFloat() - 0.5f)) * secondNormal + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // construct a ray from first point to almost second point - RayIntersectionInfo intersection; - intersection._rayStart = rayStart; - intersection._rayDirection = glm::normalize(facePoint - rayStart); - intersection._rayLength = (1.0f - SOME_SMALL_NUMBER) * glm::distance(rayStart, facePoint); - - // cast the ray - QCOMPARE(cube.findRayIntersection(intersection), false); - } - } - } - } - { // long ray misses cube - for (int i = 0; i < numDirections; ++i) { - for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) { - glm::vec3 faceNormal = sign * faceNormals[i]; - glm::vec3 secondNormal = faceNormals[(i + 1) % numDirections]; - glm::vec3 thirdNormal = faceNormals[(i + 2) % numDirections]; - - // pick a random point somewhere above the face - glm::vec3 rayStart = cubeCenter + - (cubeSide + rayOffset) * faceNormal + - (cubeSide * (randFloat() - 0.5f)) * secondNormal + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // cast multiple rays that miss the face - for (int j = 0; j < numRayCasts; ++j) { - // pick a random point just outside of face - float inside = (cubeSide * (randFloat() - 0.5f)); - float outside = 0.5f * cubeSide + SOME_SMALL_NUMBER * randFloat(); - if (randFloat() - 0.5f < 0.0f) { - outside *= -1.0f; - } - glm::vec3 sidePoint = cubeCenter + 0.5f * cubeSide * faceNormal; - if (randFloat() - 0.5f < 0.0f) { - sidePoint += outside * secondNormal + inside * thirdNormal; - } else { - sidePoint += inside * secondNormal + outside * thirdNormal; - } - - // construct a ray from first point through second point - RayIntersectionInfo intersection; - intersection._rayStart = rayStart; - intersection._rayDirection = glm::normalize(sidePoint - rayStart); - - // cast the ray - QCOMPARE(cube.findRayIntersection(intersection), false); - } - } - } - } - { // ray parallel to face barely misses cube - for (int i = 0; i < numDirections; ++i) { - for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) { - glm::vec3 faceNormal = sign * faceNormals[i]; - glm::vec3 secondNormal = faceNormals[(i + 1) % numDirections]; - glm::vec3 thirdNormal = faceNormals[(i + 2) % numDirections]; - - // cast multiple rays that miss the face - for (int j = 0; j < numRayCasts; ++j) { - // rayStart is above the face - glm::vec3 rayStart = cubeCenter + (0.5f + SOME_SMALL_NUMBER) * cubeSide * faceNormal; - - // move rayStart to some random edge and choose the ray direction to point across the face - float inside = (cubeSide * (randFloat() - 0.5f)); - float outside = 0.5f * cubeSide + SOME_SMALL_NUMBER * randFloat(); - if (randFloat() - 0.5f < 0.0f) { - outside *= -1.0f; - } - glm::vec3 rayDirection = secondNormal; - if (randFloat() - 0.5f < 0.0f) { - rayStart += outside * secondNormal + inside * thirdNormal; - } else { - rayStart += inside * secondNormal + outside * thirdNormal; - rayDirection = thirdNormal; - } - if (outside > 0.0f) { - rayDirection *= -1.0f; - } - - // construct a ray from first point through second point - RayIntersectionInfo intersection; - intersection._rayStart = rayStart; - intersection._rayDirection = rayDirection; - - // cast the ray - QCOMPARE(cube.findRayIntersection(intersection), false); - } - } - } - } - -} - -void ShapeColliderTests::measureTimeOfCollisionDispatch() { - - // TODO: use QBENCHMARK for this - - /* KEEP for future manual testing - // create two non-colliding spheres - float radiusA = 7.0f; - float radiusB = 3.0f; - float alpha = 1.2f; - float beta = 1.3f; - glm::vec3 offsetDirection = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f)); - float offsetDistance = alpha * radiusA + beta * radiusB; - - SphereShape sphereA(radiusA, origin); - SphereShape sphereB(radiusB, offsetDistance * offsetDirection); - CollisionList collisions(16); - - //int numTests = 1; - quint64 oldTime; - quint64 newTime; - int numTests = 100000000; - { - quint64 startTime = usecTimestampNow(); - for (int i = 0; i < numTests; ++i) { - ShapeCollider::collideShapes(&sphereA, &sphereB, collisions); - } - quint64 endTime = usecTimestampNow(); - std::cout << numTests << " non-colliding collisions in " << (endTime - startTime) << " usec" << std::endl; - newTime = endTime - startTime; - } - */ -} - diff --git a/tests/physics/src/ShapeColliderTests.h b/tests/physics/src/ShapeColliderTests.h deleted file mode 100644 index 73e2b972a9..0000000000 --- a/tests/physics/src/ShapeColliderTests.h +++ /dev/null @@ -1,53 +0,0 @@ -// -// ShapeColliderTests.h -// tests/physics/src -// -// Created by Andrew Meadows on 02/21/2014. -// Copyright 2014 High Fidelity, Inc. -// -// Distributed under the Apache License, Version 2.0. -// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html -// - -#ifndef hifi_ShapeColliderTests_h -#define hifi_ShapeColliderTests_h - -#include - -class ShapeColliderTests : public QObject { - Q_OBJECT - -private slots: - void initTestCase(); - - void sphereMissesSphere(); - void sphereTouchesSphere(); - - void sphereMissesCapsule(); - void sphereTouchesCapsule(); - - void capsuleMissesCapsule(); - void capsuleTouchesCapsule(); - - void sphereTouchesAACubeFaces(); - void sphereTouchesAACubeEdges(); - void sphereTouchesAACubeCorners(); - void sphereMissesAACube(); - - void capsuleMissesAACube(); - void capsuleTouchesAACube(); - - void rayHitsSphere(); - void rayBarelyHitsSphere(); - void rayBarelyMissesSphere(); - void rayHitsCapsule(); - void rayMissesCapsule(); - void rayHitsPlane(); - void rayMissesPlane(); - void rayHitsAACube(); - void rayMissesAACube(); - - void measureTimeOfCollisionDispatch(); -}; - -#endif // hifi_ShapeColliderTests_h From 4a8baafdd206f14f18b5358f64df92de050777d0 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 10 Aug 2015 14:45:57 -0700 Subject: [PATCH 02/10] fix QCOMPARE_WITH_ABS_ERROR for floats --- tests/QTestExtensions.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/tests/QTestExtensions.h b/tests/QTestExtensions.h index c374a9a507..c034e9c290 100644 --- a/tests/QTestExtensions.h +++ b/tests/QTestExtensions.h @@ -57,10 +57,10 @@ QString QTest_generateCompareFailureMessage ( QString s1 = actual_expr, s2 = expected_expr; int pad1_ = qMax(s2.length() - s1.length(), 0); int pad2_ = qMax(s1.length() - s2.length(), 0); - + QString pad1 = QString(")").rightJustified(pad1_, ' '); QString pad2 = QString(")").rightJustified(pad2_, ' '); - + QString msg; QTextStream stream (&msg); stream << failMessage << "\n\t" @@ -88,10 +88,10 @@ QString QTest_generateCompareFailureMessage ( QString s1 = actual_expr, s2 = expected_expr; int pad1_ = qMax(s2.length() - s1.length(), 0); int pad2_ = qMax(s1.length() - s2.length(), 0); - + QString pad1 = QString("): ").rightJustified(pad1_, ' '); QString pad2 = QString("): ").rightJustified(pad2_, ' '); - + QString msg; QTextStream stream (&msg); stream << failMessage << "\n\t" @@ -168,7 +168,7 @@ bool QTest_compareWithAbsError( int line, const char* file, const V& epsilon ) { - if (abs(getErrorDifference(actual, expected)) > abs(epsilon)) { + if (fabsf(getErrorDifference(actual, expected)) > fabsf(epsilon)) { QTest_failWithMessage( "Compared values are not the same (fuzzy compare)", actual, expected, actual_expr, expected_expr, line, file, @@ -260,7 +260,7 @@ do { \ struct ByteData { - ByteData (const char* data, size_t length) + ByteData (const char* data, size_t length) : data(data), length(length) {} const char* data; size_t length; From f9a4b82eddfe386f39d1440a8f6c3653390ebe3f Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 10 Aug 2015 14:47:15 -0700 Subject: [PATCH 03/10] 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); From 43bf4a85d29548a268033b0b5460ae389e29e65a Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 10 Aug 2015 14:48:22 -0700 Subject: [PATCH 04/10] add RotationConstraint and friends --- libraries/animation/src/ElbowConstraint.cpp | 80 +++++++++++ libraries/animation/src/ElbowConstraint.h | 30 ++++ libraries/animation/src/RotationConstraint.h | 29 ++++ .../animation/src/SwingTwistConstraint.cpp | 128 ++++++++++++++++++ .../animation/src/SwingTwistConstraint.h | 79 +++++++++++ 5 files changed, 346 insertions(+) create mode 100644 libraries/animation/src/ElbowConstraint.cpp create mode 100644 libraries/animation/src/ElbowConstraint.h create mode 100644 libraries/animation/src/RotationConstraint.h create mode 100644 libraries/animation/src/SwingTwistConstraint.cpp create mode 100644 libraries/animation/src/SwingTwistConstraint.h diff --git a/libraries/animation/src/ElbowConstraint.cpp b/libraries/animation/src/ElbowConstraint.cpp new file mode 100644 index 0000000000..e575b3f8b7 --- /dev/null +++ b/libraries/animation/src/ElbowConstraint.cpp @@ -0,0 +1,80 @@ +// +// ElbowConstraint.cpp +// +// Copyright 2015 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include "ElbowConstraint.h" + +#include + +#include +#include + +ElbowConstraint::ElbowConstraint() : + _referenceRotation(), + _minAngle(-PI), + _maxAngle(PI) +{ +} + +void ElbowConstraint::setHingeAxis(const glm::vec3& axis) { + float axisLength = glm::length(axis); + assert(axisLength > EPSILON); + _axis = axis / axisLength; + + // for later we need a second axis that is perpendicular to the first + for (int i = 0; i < 3; ++i) { + float component = _axis[i]; + const float MIN_LARGEST_NORMALIZED_VECTOR_COMPONENT = 0.57735f; // just under 1/sqrt(3) + if (fabsf(component) > MIN_LARGEST_NORMALIZED_VECTOR_COMPONENT) { + int j = (i + 1) % 3; + int k = (j + 1) % 3; + _perpAxis[i] = - _axis[j]; + _perpAxis[j] = component; + _perpAxis[k] = 0.0f; + _perpAxis = glm::normalize(_perpAxis); + break; + } + } +} + +void ElbowConstraint::setAngleLimits(float minAngle, float maxAngle) { + // NOTE: min/maxAngle angles should be in the range [-PI, PI] + _minAngle = glm::min(minAngle, maxAngle); + _maxAngle = glm::max(minAngle, maxAngle); +} + +bool ElbowConstraint::apply(glm::quat& rotation) const { + // decompose the rotation into swing and twist + // NOTE: rotation = postRotation * referenceRotation + glm::quat postRotation = rotation * glm::inverse(_referenceRotation); + glm::quat swingRotation, twistRotation; + swingTwistDecomposition(postRotation, _axis, swingRotation, twistRotation); + // NOTE: postRotation = swingRotation * twistRotation + + // compute twistAngle + float twistAngle = 2.0f * acosf(fabsf(twistRotation.w)); + glm::vec3 twisted = twistRotation * _perpAxis; + twistAngle *= copysignf(1.0f, glm::dot(glm::cross(_perpAxis, twisted), _axis)); + + // clamp twistAngle + float clampedTwistAngle = std::max(_minAngle, std::min(twistAngle, _maxAngle)); + bool twistWasClamped = (twistAngle != clampedTwistAngle); + + // update rotation + const float MIN_SWING_REAL_PART = 0.99999f; + if (twistWasClamped || fabsf(swingRotation.w < MIN_SWING_REAL_PART)) { + if (twistWasClamped) { + twistRotation = glm::angleAxis(clampedTwistAngle, _axis); + } + // we discard all swing and only keep twist + rotation = twistRotation * _referenceRotation; + return true; + } + return false; +} + diff --git a/libraries/animation/src/ElbowConstraint.h b/libraries/animation/src/ElbowConstraint.h new file mode 100644 index 0000000000..3c4481cfa8 --- /dev/null +++ b/libraries/animation/src/ElbowConstraint.h @@ -0,0 +1,30 @@ +// +// ElbowConstraint.h +// +// Copyright 2015 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_ElbowConstraint_h +#define hifi_ElbowConstraint_h + +#include "RotationConstraint.h" + +class ElbowConstraint : public RotationConstraint { +public: + ElbowConstraint(); + virtual void setReferenceRotation(const glm::quat& rotation) override { _referenceRotation = rotation; } + void setHingeAxis(const glm::vec3& axis); + void setAngleLimits(float minAngle, float maxAngle); + virtual bool apply(glm::quat& rotation) const override; +protected: + glm::quat _referenceRotation; + glm::vec3 _axis; + glm::vec3 _perpAxis; + float _minAngle; + float _maxAngle; +}; + +#endif // hifi_ElbowConstraint_h diff --git a/libraries/animation/src/RotationConstraint.h b/libraries/animation/src/RotationConstraint.h new file mode 100644 index 0000000000..e664a9d8af --- /dev/null +++ b/libraries/animation/src/RotationConstraint.h @@ -0,0 +1,29 @@ +// +// RotationConstrain.h +// +// Copyright 2015 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_RotationConstraint_h +#define hifi_RotationConstraint_h + +#include +#include + +class RotationConstraint { +public: + RotationConstraint() {} + virtual ~RotationConstraint() {} + + /// \param rotation the default rotation that represents + virtual void setReferenceRotation(const glm::quat& rotation) = 0; + + /// \param rotation rotation to clamp + /// \return true if rotation is clamped + virtual bool apply(glm::quat& rotation) const = 0; +}; + +#endif // hifi_RotationConstraint_h diff --git a/libraries/animation/src/SwingTwistConstraint.cpp b/libraries/animation/src/SwingTwistConstraint.cpp new file mode 100644 index 0000000000..9c88b31637 --- /dev/null +++ b/libraries/animation/src/SwingTwistConstraint.cpp @@ -0,0 +1,128 @@ +// +// SwingTwistConstraint.cpp +// +// Copyright 2015 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include "SwingTwistConstraint.h" + +#include +#include + +#include +#include + + +const float MIN_MINDOT = -0.999f; +const float MAX_MINDOT = 1.0f; + +SwingTwistConstraint::SwingLimitFunction::SwingLimitFunction() { + setCone(PI); +} + +void SwingTwistConstraint::SwingLimitFunction::setCone(float maxAngle) { + _minDots.clear(); + float minDot = std::max(MIN_MINDOT, std::min(cosf(maxAngle), MAX_MINDOT)); + _minDots.push_back(minDot); + // push the first value to the back to establish cyclic boundary conditions + _minDots.push_back(minDot); +} + +void SwingTwistConstraint::SwingLimitFunction::setMinDots(const std::vector& minDots) { + int numDots = minDots.size(); + _minDots.clear(); + _minDots.reserve(numDots); + for (int i = 0; i < numDots; ++i) { + _minDots.push_back(std::max(MIN_MINDOT, std::min(minDots[i], MAX_MINDOT))); + } + // push the first value to the back to establish cyclic boundary conditions + _minDots.push_back(_minDots[0]); +} + +float SwingTwistConstraint::SwingLimitFunction::getMinDot(float theta) const { + // extract the positive normalized fractional part of theta + float integerPart; + float normalizedTheta = modff(theta / TWO_PI, &integerPart); + if (normalizedTheta < 0.0f) { + normalizedTheta += 1.0f; + } + + // interpolate between the two nearest points in the cycle + float fractionPart = modff(normalizedTheta * (float)(_minDots.size() - 1), &integerPart); + int i = (int)(integerPart); + int j = (i + 1) % _minDots.size(); + return _minDots[i] * (1.0f - fractionPart) + _minDots[j] * fractionPart; +} + +SwingTwistConstraint::SwingTwistConstraint() : + _swingLimitFunction(), + _referenceRotation(), + _minTwist(-PI), + _maxTwist(PI) +{ +} + +void SwingTwistConstraint::setSwingLimits(std::vector minDots) { + _swingLimitFunction.setMinDots(minDots); +} + +void SwingTwistConstraint::setTwistLimits(float minTwist, float maxTwist) { + // NOTE: min/maxTwist angles should be in the range [-PI, PI] + _minTwist = std::min(minTwist, maxTwist); + _maxTwist = std::max(minTwist, maxTwist); +} + +bool SwingTwistConstraint::apply(glm::quat& rotation) const { + + // decompose the rotation into first twist about yAxis, then swing about something perp + const glm::vec3 yAxis(0.0f, 1.0f, 0.0f); + // NOTE: rotation = postRotation * referenceRotation + glm::quat postRotation = rotation * glm::inverse(_referenceRotation); + glm::quat swingRotation, twistRotation; + swingTwistDecomposition(postRotation, yAxis, swingRotation, twistRotation); + // NOTE: postRotation = swingRotation * twistRotation + + // compute twistAngle + float twistAngle = 2.0f * acosf(fabsf(twistRotation.w)); + const glm::vec3 xAxis = glm::vec3(1.0f, 0.0f, 0.0f); + glm::vec3 twistedX = twistRotation * xAxis; + twistAngle *= copysignf(1.0f, glm::dot(glm::cross(xAxis, twistedX), yAxis)); + + // clamp twistAngle + float clampedTwistAngle = std::max(_minTwist, std::min(twistAngle, _maxTwist)); + bool twistWasClamped = (twistAngle != clampedTwistAngle); + + // clamp the swing + // The swingAxis is always perpendicular to the reference axis (yAxis in the constraint's frame). + glm::vec3 swungY = swingRotation * yAxis; + glm::vec3 swingAxis = glm::cross(yAxis, swungY); + bool swingWasClamped = false; + float axisLength = glm::length(swingAxis); + if (axisLength > EPSILON) { + // The limit of swing is a function of "theta" which can be computed from the swingAxis + // (which is in the constraint's ZX plane). + float theta = atan2f(-swingAxis.z, swingAxis.x); + float minDot = _swingLimitFunction.getMinDot(theta); + if (glm::dot(swungY, yAxis) < minDot) { + // The swing limits are violated so we use the maxAngle to supply a new rotation. + float maxAngle = acosf(minDot); + if (minDot < 0.0f) { + maxAngle = PI - maxAngle; + } + swingAxis /= axisLength; + swingRotation = glm::angleAxis(maxAngle, swingAxis); + swingWasClamped = true; + } + } + + if (swingWasClamped || twistWasClamped) { + twistRotation = glm::angleAxis(clampedTwistAngle, yAxis); + rotation = swingRotation * twistRotation * _referenceRotation; + return true; + } + return false; +} + diff --git a/libraries/animation/src/SwingTwistConstraint.h b/libraries/animation/src/SwingTwistConstraint.h new file mode 100644 index 0000000000..c5e6d9e569 --- /dev/null +++ b/libraries/animation/src/SwingTwistConstraint.h @@ -0,0 +1,79 @@ +// +// SwingTwistConstraint.h +// +// Copyright 2015 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_SwingTwistConstraint_h +#define hifi_SwingTwistConstraint_h + +#include + +#include "RotationConstraint.h" + +#include + +class SwingTwistConstraint : RotationConstraint { +public: + // The SwingTwistConstraint starts in the "referenceRotation" and then measures an initial twist + // about the yAxis followed by a swing about some axis that lies in the XZ plane, such that the twist + // and swing combine to produce the rotation. Each partial rotation is constrained within limits + // then used to construct the new final rotation. + + SwingTwistConstraint(); + + /// \param referenceRotation the rotation from which rotation changes are measured. + virtual void setReferenceRotation(const glm::quat& referenceRotation) override { _referenceRotation = referenceRotation; } + + /// \param minDots vector of minimum dot products between the twist and swung axes. + /// \brief The values are minimum dot-products between the twist axis and the swung axes + /// that correspond to swing axes equally spaced around the XZ plane. Another way to + /// think about it is that the dot-products correspond to correspond to angles (theta) + /// about the twist axis ranging from 0 to 2PI-deltaTheta (Note: the cyclic boundary + /// conditions are handled internally, so don't duplicate the dot-product at 2PI). + /// See the paper by Quang Liu and Edmond C. Prakash mentioned below for a more detailed + /// description of how this works. + void setSwingLimits(std::vector minDots); + + /// \param minTwist the minimum angle of rotation about the twist axis + /// \param maxTwist the maximum angle of rotation about the twist axis + void setTwistLimits(float minTwist, float maxTwist); + + /// \param rotation[in/out] the current rotation to be modified to fit within constraints + /// \return true if rotation is changed + virtual bool apply(glm::quat& rotation) const override; + + // SwingLimitFunction is an implementation of the constraint check described in the paper: + // "The Parameterization of Joint Rotation with the Unit Quaternion" by Quang Liu and Edmond C. Prakash + class SwingLimitFunction { + public: + SwingLimitFunction(); + + /// \brief use a uniform conical swing limit + void setCone(float maxAngle); + + /// \brief use a vector of lookup values for swing limits + void setMinDots(const std::vector& minDots); + + /// \return minimum dotProduct between reference and swung axes + float getMinDot(float theta) const; + + protected: + // the limits are stored in a lookup table with cyclic boundary conditions + std::vector _minDots; + }; + + /// \return reference to SwingLimitFunction instance for unit-testing + const SwingLimitFunction& getSwingLimitFunction() const { return _swingLimitFunction; } + +protected: + SwingLimitFunction _swingLimitFunction; + glm::quat _referenceRotation; + float _minTwist; + float _maxTwist; +}; + +#endif // hifi_SwingTwistConstraint_h From 75ab6a00aa9fad0752c5680dbaf3328ebe936aa0 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 10 Aug 2015 14:48:46 -0700 Subject: [PATCH 05/10] add unit tests for RotationConstraints --- .../animation/src/RotationConstraintTests.cpp | 331 ++++++++++++++++++ tests/animation/src/RotationConstraintTests.h | 24 ++ 2 files changed, 355 insertions(+) create mode 100644 tests/animation/src/RotationConstraintTests.cpp create mode 100644 tests/animation/src/RotationConstraintTests.h diff --git a/tests/animation/src/RotationConstraintTests.cpp b/tests/animation/src/RotationConstraintTests.cpp new file mode 100644 index 0000000000..c98711e7a5 --- /dev/null +++ b/tests/animation/src/RotationConstraintTests.cpp @@ -0,0 +1,331 @@ +// +// RotationConstraintTests.cpp +// tests/rig/src +// +// Copyright 2015 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include "RotationConstraintTests.h" + +#include + +#include +#include +#include + +// HACK -- these helper functions need to be defined BEFORE including magic inside QTestExtensions.h +// TODO: fix QTestExtensions so we don't need to do this in every test. + +// Computes the error value between two quaternions (using glm::dot) +float getErrorDifference(const glm::quat& a, const glm::quat& b) { + return fabsf(glm::dot(a, b)) - 1.0f; +} + +QTextStream& operator<<(QTextStream& stream, const glm::quat& q) { + return stream << "glm::quat { " << q.x << ", " << q.y << ", " << q.z << ", " << q.w << " }"; +} + +// Produces a relative error test for float usable QCOMPARE_WITH_LAMBDA. +inline auto errorTest (float actual, float expected, float acceptableRelativeError) +-> std::function { + return [&actual, &expected, acceptableRelativeError] () { + if (expected <= acceptableRelativeError) { + return fabsf(actual - expected) < acceptableRelativeError; + } + return fabsf(actual - expected) / expected < acceptableRelativeError; + }; +} + +#include "../QTestExtensions.h" + +#define QCOMPARE_WITH_RELATIVE_ERROR(actual, expected, relativeError) \ + QCOMPARE_WITH_LAMBDA(actual, expected, errorTest(actual, expected, relativeError)) + + +QTEST_MAIN(RotationConstraintTests) + +void RotationConstraintTests::testElbowConstraint() { + // referenceRotation is the default rotation + float referenceAngle = 1.23f; + glm::vec3 referenceAxis = glm::normalize(glm::vec3(1.0f, 2.0f, -3.0f)); + glm::quat referenceRotation = glm::angleAxis(referenceAngle, referenceAxis); + + // NOTE: hingeAxis is in the "referenceFrame" + glm::vec3 hingeAxis = glm::vec3(1.0f, 0.0f, 0.0f); + + // the angle limits of the constriant about the hinge axis + float minAngle = -PI / 4.0f; + float maxAngle = PI / 3.0f; + + // build the constraint + ElbowConstraint elbow; + elbow.setReferenceRotation(referenceRotation); + elbow.setHingeAxis(hingeAxis); + elbow.setAngleLimits(minAngle, maxAngle); + + float smallAngle = PI / 100.0f; + + { // test reference rotation -- should be unconstrained + glm::quat inputRotation = referenceRotation; + glm::quat outputRotation = inputRotation; + bool updated = elbow.apply(outputRotation); + QVERIFY(updated == false); + glm::quat expectedRotation = referenceRotation; + QCOMPARE_WITH_ABS_ERROR(expectedRotation, outputRotation, EPSILON); + } + + { // test several simple rotations that are INSIDE the limits -- should be unconstrained + int numChecks = 10; + float startAngle = minAngle + smallAngle; + float endAngle = maxAngle - smallAngle; + float deltaAngle = (endAngle - startAngle) / (float)(numChecks - 1); + + for (float angle = startAngle; angle < endAngle + 0.5f * deltaAngle; angle += deltaAngle) { + glm::quat inputRotation = glm::angleAxis(angle, hingeAxis) * referenceRotation; + glm::quat outputRotation = inputRotation; + bool updated = elbow.apply(outputRotation); + QVERIFY(updated == false); + QCOMPARE_WITH_ABS_ERROR(inputRotation, outputRotation, EPSILON); + } + } + + { // test simple rotation just OUTSIDE minAngle -- should be constrained + float angle = minAngle - smallAngle; + glm::quat inputRotation = glm::angleAxis(angle, hingeAxis) * referenceRotation; + glm::quat outputRotation = inputRotation; + bool updated = elbow.apply(outputRotation); + QVERIFY(updated == true); + glm::quat expectedRotation = glm::angleAxis(minAngle, hingeAxis) * referenceRotation; + QCOMPARE_WITH_ABS_ERROR(expectedRotation, outputRotation, EPSILON); + } + + { // test simple rotation just OUTSIDE maxAngle -- should be constrained + float angle = maxAngle + smallAngle; + glm::quat inputRotation = glm::angleAxis(angle, hingeAxis) * referenceRotation; + glm::quat outputRotation = inputRotation; + bool updated = elbow.apply(outputRotation); + QVERIFY(updated == true); + glm::quat expectedRotation = glm::angleAxis(maxAngle, hingeAxis) * referenceRotation; + QCOMPARE_WITH_ABS_ERROR(expectedRotation, outputRotation, EPSILON); + } + + { // test simple twist rotation that has no hinge component -- should be constrained + glm::vec3 someVector(7.0f, -5.0f, 2.0f); + glm::vec3 twistVector = glm::normalize(glm::cross(hingeAxis, someVector)); + float someAngle = 0.789f; + glm::quat inputRotation = glm::angleAxis(someAngle, twistVector) * referenceRotation; + glm::quat outputRotation = inputRotation; + bool updated = elbow.apply(outputRotation); + QVERIFY(updated == true); + glm::quat expectedRotation = referenceRotation; + QCOMPARE_WITH_ABS_ERROR(expectedRotation, outputRotation, EPSILON); + } +} + +void RotationConstraintTests::testSwingTwistConstraint() { + // referenceRotation is the default rotation + float referenceAngle = 1.23f; + glm::vec3 referenceAxis = glm::normalize(glm::vec3(1.0f, 2.0f, -3.0f)); + glm::quat referenceRotation = glm::angleAxis(referenceAngle, referenceAxis); + + // the angle limits of the constriant about the hinge axis + float minTwistAngle = -PI / 2.0f; + float maxTwistAngle = PI / 2.0f; + + // build the constraint + SwingTwistConstraint shoulder; + shoulder.setReferenceRotation(referenceRotation); + shoulder.setTwistLimits(minTwistAngle, maxTwistAngle); + float lowDot = 0.25f; + float highDot = 0.75f; + // The swing constriants are more interesting: a vector of minimum dot products + // as a function of theta around the twist axis. Our test function will be shaped + // like the square wave with amplitudes 0.25 and 0.75: + // + // | + // 0.75 - o---o---o---o + // | / ' + // | / ' + // | / ' + // 0.25 o---o---o---o o + // | + // +-------+-------+-------+-------+--- + // 0 pi/2 pi 3pi/2 2pi + + int numDots = 8; + std::vector minDots; + int dotIndex = 0; + while (dotIndex < numDots / 2) { + ++dotIndex; + minDots.push_back(lowDot); + } + while (dotIndex < numDots) { + minDots.push_back(highDot); + ++dotIndex; + } + shoulder.setSwingLimits(minDots); + const SwingTwistConstraint::SwingLimitFunction& shoulderSwingLimitFunction = shoulder.getSwingLimitFunction(); + + { // test interpolation of SwingLimitFunction + float theta = 0.0f; + float minDot = shoulderSwingLimitFunction.getMinDot(theta); + float expectedMinDot = lowDot; + QCOMPARE_WITH_RELATIVE_ERROR(minDot, expectedMinDot, EPSILON); + + theta = PI; + minDot = shoulderSwingLimitFunction.getMinDot(theta); + expectedMinDot = highDot; + QCOMPARE_WITH_RELATIVE_ERROR(minDot, expectedMinDot, EPSILON); + + // test interpolation on upward slope + theta = PI * (7.0f / 8.0f); + minDot = shoulderSwingLimitFunction.getMinDot(theta); + expectedMinDot = 0.5f * (highDot + lowDot); + QCOMPARE_WITH_RELATIVE_ERROR(minDot, expectedMinDot, EPSILON); + + // test interpolation on downward slope + theta = PI * (15.0f / 8.0f); + minDot = shoulderSwingLimitFunction.getMinDot(theta); + expectedMinDot = 0.5f * (highDot + lowDot); + } + + float smallAngle = PI / 100.0f; + + // Note: the twist is always about the yAxis + glm::vec3 yAxis(0.0f, 1.0f, 0.0f); + + { // test INSIDE both twist and swing + int numSwingAxes = 7; + float deltaTheta = TWO_PI / numSwingAxes; + + int numTwists = 2; + float startTwist = minTwistAngle + smallAngle; + float endTwist = maxTwistAngle - smallAngle; + float deltaTwist = (endTwist - startTwist) / (float)(numTwists - 1); + float twist = startTwist; + + for (int i = 0; i < numTwists; ++i) { + glm::quat twistRotation = glm::angleAxis(twist, yAxis); + + for (float theta = 0.0f; theta < TWO_PI; theta += deltaTheta) { + float swing = acosf(shoulderSwingLimitFunction.getMinDot(theta)) - smallAngle; + glm::vec3 swingAxis(cosf(theta), 0.0f, -sinf(theta)); + glm::quat swingRotation = glm::angleAxis(swing, swingAxis); + + glm::quat inputRotation = swingRotation * twistRotation * referenceRotation; + glm::quat outputRotation = inputRotation; + + bool updated = shoulder.apply(outputRotation); + QVERIFY(updated == false); + QCOMPARE_WITH_ABS_ERROR(inputRotation, outputRotation, EPSILON); + } + twist += deltaTwist; + } + } + + { // test INSIDE twist but OUTSIDE swing + int numSwingAxes = 7; + float deltaTheta = TWO_PI / numSwingAxes; + + int numTwists = 2; + float startTwist = minTwistAngle + smallAngle; + float endTwist = maxTwistAngle - smallAngle; + float deltaTwist = (endTwist - startTwist) / (float)(numTwists - 1); + float twist = startTwist; + + for (int i = 0; i < numTwists; ++i) { + glm::quat twistRotation = glm::angleAxis(twist, yAxis); + + for (float theta = 0.0f; theta < TWO_PI; theta += deltaTheta) { + float maxSwingAngle = acosf(shoulderSwingLimitFunction.getMinDot(theta)); + float swing = maxSwingAngle + smallAngle; + glm::vec3 swingAxis(cosf(theta), 0.0f, -sinf(theta)); + glm::quat swingRotation = glm::angleAxis(swing, swingAxis); + + glm::quat inputRotation = swingRotation * twistRotation * referenceRotation; + glm::quat outputRotation = inputRotation; + + bool updated = shoulder.apply(outputRotation); + QVERIFY(updated == true); + + glm::quat expectedSwingRotation = glm::angleAxis(maxSwingAngle, swingAxis); + glm::quat expectedRotation = expectedSwingRotation * twistRotation * referenceRotation; + QCOMPARE_WITH_ABS_ERROR(expectedRotation, outputRotation, EPSILON); + } + twist += deltaTwist; + } + } + + { // test OUTSIDE twist but INSIDE swing + int numSwingAxes = 6; + float deltaTheta = TWO_PI / numSwingAxes; + + int numTwists = 2; + float startTwist = minTwistAngle - smallAngle; + float endTwist = maxTwistAngle + smallAngle; + float deltaTwist = (endTwist - startTwist) / (float)(numTwists - 1); + float twist = startTwist; + + for (int i = 0; i < numTwists; ++i) { + glm::quat twistRotation = glm::angleAxis(twist, yAxis); + float clampedTwistAngle = std::min(maxTwistAngle, std::max(minTwistAngle, twist)); + + for (float theta = 0.0f; theta < TWO_PI; theta += deltaTheta) { + float maxSwingAngle = acosf(shoulderSwingLimitFunction.getMinDot(theta)); + float swing = maxSwingAngle - smallAngle; + glm::vec3 swingAxis(cosf(theta), 0.0f, -sinf(theta)); + glm::quat swingRotation = glm::angleAxis(swing, swingAxis); + + glm::quat inputRotation = swingRotation * twistRotation * referenceRotation; + glm::quat outputRotation = inputRotation; + + bool updated = shoulder.apply(outputRotation); + QVERIFY(updated == true); + + glm::quat expectedTwistRotation = glm::angleAxis(clampedTwistAngle, yAxis); + glm::quat expectedRotation = swingRotation * expectedTwistRotation * referenceRotation; + QCOMPARE_WITH_ABS_ERROR(expectedRotation, outputRotation, EPSILON); + } + twist += deltaTwist; + } + } + + { // test OUTSIDE both twist and swing + int numSwingAxes = 5; + float deltaTheta = TWO_PI / numSwingAxes; + + int numTwists = 2; + float startTwist = minTwistAngle - smallAngle; + float endTwist = maxTwistAngle + smallAngle; + float deltaTwist = (endTwist - startTwist) / (float)(numTwists - 1); + float twist = startTwist; + + for (int i = 0; i < numTwists; ++i) { + glm::quat twistRotation = glm::angleAxis(twist, yAxis); + float clampedTwistAngle = std::min(maxTwistAngle, std::max(minTwistAngle, twist)); + + for (float theta = 0.0f; theta < TWO_PI; theta += deltaTheta) { + float maxSwingAngle = acosf(shoulderSwingLimitFunction.getMinDot(theta)); + float swing = maxSwingAngle + smallAngle; + glm::vec3 swingAxis(cosf(theta), 0.0f, -sinf(theta)); + glm::quat swingRotation = glm::angleAxis(swing, swingAxis); + + glm::quat inputRotation = swingRotation * twistRotation * referenceRotation; + glm::quat outputRotation = inputRotation; + + bool updated = shoulder.apply(outputRotation); + QVERIFY(updated == true); + + glm::quat expectedTwistRotation = glm::angleAxis(clampedTwistAngle, yAxis); + glm::quat expectedSwingRotation = glm::angleAxis(maxSwingAngle, swingAxis); + glm::quat expectedRotation = expectedSwingRotation * expectedTwistRotation * referenceRotation; + QCOMPARE_WITH_ABS_ERROR(expectedRotation, outputRotation, EPSILON); + } + twist += deltaTwist; + } + } +} + diff --git a/tests/animation/src/RotationConstraintTests.h b/tests/animation/src/RotationConstraintTests.h new file mode 100644 index 0000000000..4fed3588e4 --- /dev/null +++ b/tests/animation/src/RotationConstraintTests.h @@ -0,0 +1,24 @@ +// +// RotationConstraintTests.h +// tests/rig/src +// +// Copyright 2015 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_RotationConstraintTests_h +#define hifi_RotationConstraintTests_h + +#include + +class RotationConstraintTests : public QObject { + Q_OBJECT + +private slots: + void testElbowConstraint(); + void testSwingTwistConstraint(); +}; + +#endif // hifi_RotationConstraintTests_h From 80b36f9f12bbff5e6289309f3cf62952e494d5e6 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 10 Aug 2015 15:11:00 -0700 Subject: [PATCH 06/10] fix typo --- libraries/animation/src/RotationConstraint.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/animation/src/RotationConstraint.h b/libraries/animation/src/RotationConstraint.h index e664a9d8af..a05c0d0526 100644 --- a/libraries/animation/src/RotationConstraint.h +++ b/libraries/animation/src/RotationConstraint.h @@ -1,5 +1,5 @@ // -// RotationConstrain.h +// RotationConstraint.h // // Copyright 2015 High Fidelity, Inc. // From 4655bd21dd14c06a6d06ef65d395bb239255b861 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 10 Aug 2015 23:08:10 -0700 Subject: [PATCH 07/10] added some comments to clarify algorithm --- libraries/shared/src/GeometryUtil.cpp | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/libraries/shared/src/GeometryUtil.cpp b/libraries/shared/src/GeometryUtil.cpp index acc112bfd6..7d8ee185d5 100644 --- a/libraries/shared/src/GeometryUtil.cpp +++ b/libraries/shared/src/GeometryUtil.cpp @@ -9,11 +9,13 @@ // See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html // +#include "GeometryUtil.h" + +#include #include #include #include -#include "GeometryUtil.h" #include "NumericalConstants.h" glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const glm::vec3& start, const glm::vec3& end) { @@ -539,11 +541,19 @@ bool findRayRectangleIntersection(const glm::vec3& origin, const glm::vec3& dire } void swingTwistDecomposition(const glm::quat& rotation, - const glm::vec3& direction, // must be normalized + const glm::vec3& direction, 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)); + // direction MUST be normalized else the decomposition will be inaccurate + assert(fabsf(glm::length2(direction) - 1.0f) < 1.0e-4f); + + // the twist part has an axis (imaginary component) that is parallel to direction argument + glm::vec3 axisOfRotation(rotation.x, rotation.y, rotation.z); + glm::vec3 twistImaginaryPart = glm::dot(direction, axisOfRotation) * direction; + // and a real component that is relatively proportional to rotation's real component + twist = glm::normalize(glm::quat(rotation.w, twistImaginaryPart.x, twistImaginaryPart.y, twistImaginaryPart.z)); + + // once twist is known we can solve for swing: + // rotation = swing * twist --> swing = rotation * invTwist swing = rotation * glm::inverse(twist); } From a38c1c82d1a165fd2c26bd0250fcad68f1b22f07 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 10 Aug 2015 23:08:36 -0700 Subject: [PATCH 08/10] use glm::clamp() instead if std::max() and min() --- libraries/animation/src/ElbowConstraint.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/animation/src/ElbowConstraint.cpp b/libraries/animation/src/ElbowConstraint.cpp index e575b3f8b7..8c7fec7b6f 100644 --- a/libraries/animation/src/ElbowConstraint.cpp +++ b/libraries/animation/src/ElbowConstraint.cpp @@ -62,7 +62,7 @@ bool ElbowConstraint::apply(glm::quat& rotation) const { twistAngle *= copysignf(1.0f, glm::dot(glm::cross(_perpAxis, twisted), _axis)); // clamp twistAngle - float clampedTwistAngle = std::max(_minAngle, std::min(twistAngle, _maxAngle)); + float clampedTwistAngle = glm::clamp(twistAngle, _minAngle, _maxAngle); bool twistWasClamped = (twistAngle != clampedTwistAngle); // update rotation From 23c92662ee8a4c69dbd5097424260b5db79f5bff Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 10 Aug 2015 23:09:11 -0700 Subject: [PATCH 09/10] relaxed error, more rigor for using fabsf() --- .../animation/src/RotationConstraintTests.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/tests/animation/src/RotationConstraintTests.cpp b/tests/animation/src/RotationConstraintTests.cpp index c98711e7a5..2cdb44ee8f 100644 --- a/tests/animation/src/RotationConstraintTests.cpp +++ b/tests/animation/src/RotationConstraintTests.cpp @@ -31,11 +31,11 @@ QTextStream& operator<<(QTextStream& stream, const glm::quat& q) { // Produces a relative error test for float usable QCOMPARE_WITH_LAMBDA. inline auto errorTest (float actual, float expected, float acceptableRelativeError) -> std::function { - return [&actual, &expected, acceptableRelativeError] () { - if (expected <= acceptableRelativeError) { - return fabsf(actual - expected) < acceptableRelativeError; + return [actual, expected, acceptableRelativeError] () { + if (fabsf(expected) <= acceptableRelativeError) { + return fabsf(actual - expected) < fabsf(acceptableRelativeError); } - return fabsf(actual - expected) / expected < acceptableRelativeError; + return fabsf((actual - expected) / expected) < fabsf(acceptableRelativeError); }; } @@ -170,21 +170,22 @@ void RotationConstraintTests::testSwingTwistConstraint() { const SwingTwistConstraint::SwingLimitFunction& shoulderSwingLimitFunction = shoulder.getSwingLimitFunction(); { // test interpolation of SwingLimitFunction + const float ACCEPTABLE_ERROR = 1.0e-5f; float theta = 0.0f; float minDot = shoulderSwingLimitFunction.getMinDot(theta); float expectedMinDot = lowDot; - QCOMPARE_WITH_RELATIVE_ERROR(minDot, expectedMinDot, EPSILON); + QCOMPARE_WITH_RELATIVE_ERROR(minDot, expectedMinDot, ACCEPTABLE_ERROR); theta = PI; minDot = shoulderSwingLimitFunction.getMinDot(theta); expectedMinDot = highDot; - QCOMPARE_WITH_RELATIVE_ERROR(minDot, expectedMinDot, EPSILON); + QCOMPARE_WITH_RELATIVE_ERROR(minDot, expectedMinDot, ACCEPTABLE_ERROR); // test interpolation on upward slope theta = PI * (7.0f / 8.0f); minDot = shoulderSwingLimitFunction.getMinDot(theta); expectedMinDot = 0.5f * (highDot + lowDot); - QCOMPARE_WITH_RELATIVE_ERROR(minDot, expectedMinDot, EPSILON); + QCOMPARE_WITH_RELATIVE_ERROR(minDot, expectedMinDot, ACCEPTABLE_ERROR); // test interpolation on downward slope theta = PI * (15.0f / 8.0f); @@ -271,7 +272,7 @@ void RotationConstraintTests::testSwingTwistConstraint() { for (int i = 0; i < numTwists; ++i) { glm::quat twistRotation = glm::angleAxis(twist, yAxis); - float clampedTwistAngle = std::min(maxTwistAngle, std::max(minTwistAngle, twist)); + float clampedTwistAngle = glm::clamp(twist, minTwistAngle, maxTwistAngle); for (float theta = 0.0f; theta < TWO_PI; theta += deltaTheta) { float maxSwingAngle = acosf(shoulderSwingLimitFunction.getMinDot(theta)); @@ -305,7 +306,7 @@ void RotationConstraintTests::testSwingTwistConstraint() { for (int i = 0; i < numTwists; ++i) { glm::quat twistRotation = glm::angleAxis(twist, yAxis); - float clampedTwistAngle = std::min(maxTwistAngle, std::max(minTwistAngle, twist)); + float clampedTwistAngle = glm::clamp(twist, minTwistAngle, maxTwistAngle); for (float theta = 0.0f; theta < TWO_PI; theta += deltaTheta) { float maxSwingAngle = acosf(shoulderSwingLimitFunction.getMinDot(theta)); From 8e82e48f76e89ea122c2d1fdb67fdc11c296abda Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 11 Aug 2015 08:17:54 -0700 Subject: [PATCH 10/10] remove more std::max/min favor of glm utilities --- libraries/animation/src/SwingTwistConstraint.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/animation/src/SwingTwistConstraint.cpp b/libraries/animation/src/SwingTwistConstraint.cpp index 9c88b31637..96140f4deb 100644 --- a/libraries/animation/src/SwingTwistConstraint.cpp +++ b/libraries/animation/src/SwingTwistConstraint.cpp @@ -25,7 +25,7 @@ SwingTwistConstraint::SwingLimitFunction::SwingLimitFunction() { void SwingTwistConstraint::SwingLimitFunction::setCone(float maxAngle) { _minDots.clear(); - float minDot = std::max(MIN_MINDOT, std::min(cosf(maxAngle), MAX_MINDOT)); + float minDot = glm::clamp(maxAngle, MIN_MINDOT, MAX_MINDOT); _minDots.push_back(minDot); // push the first value to the back to establish cyclic boundary conditions _minDots.push_back(minDot); @@ -36,7 +36,7 @@ void SwingTwistConstraint::SwingLimitFunction::setMinDots(const std::vector minDots) { void SwingTwistConstraint::setTwistLimits(float minTwist, float maxTwist) { // NOTE: min/maxTwist angles should be in the range [-PI, PI] - _minTwist = std::min(minTwist, maxTwist); - _maxTwist = std::max(minTwist, maxTwist); + _minTwist = glm::min(minTwist, maxTwist); + _maxTwist = glm::max(minTwist, maxTwist); } bool SwingTwistConstraint::apply(glm::quat& rotation) const { @@ -92,7 +92,7 @@ bool SwingTwistConstraint::apply(glm::quat& rotation) const { twistAngle *= copysignf(1.0f, glm::dot(glm::cross(xAxis, twistedX), yAxis)); // clamp twistAngle - float clampedTwistAngle = std::max(_minTwist, std::min(twistAngle, _maxTwist)); + float clampedTwistAngle = glm::clamp(twistAngle, _minTwist, _maxTwist); bool twistWasClamped = (twistAngle != clampedTwistAngle); // clamp the swing