From c6253bb51a0eb5265b14355248b60b6ebea49349 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 27 Aug 2014 12:09:41 -0700 Subject: [PATCH] AACube vs Sphere and Capsule collision tests Sphere is unit tested. Also removed the shape collision query against Octree --- interface/src/avatar/MyAvatar.cpp | 2 + libraries/octree/src/Octree.cpp | 4 + libraries/octree/src/Octree.h | 5 +- libraries/shared/src/ShapeCollider.cpp | 324 +++++++--------- libraries/shared/src/ShapeCollider.h | 31 +- tests/physics/src/ShapeColliderTests.cpp | 450 +++++++++++++++-------- tests/physics/src/ShapeColliderTests.h | 1 + 7 files changed, 456 insertions(+), 361 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 239ca16186..6109ad4ef6 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1564,6 +1564,7 @@ void MyAvatar::updateCollisionWithEnvironment(float deltaTime, float radius) { static CollisionList myCollisions(64); void MyAvatar::updateCollisionWithVoxels(float deltaTime, float radius) { + /* TODO: Andrew to reimplement this const float MAX_VOXEL_COLLISION_SPEED = 100.0f; float speed = glm::length(_velocity); if (speed > MAX_VOXEL_COLLISION_SPEED) { @@ -1670,6 +1671,7 @@ void MyAvatar::updateCollisionWithVoxels(float deltaTime, float radius) { //updateCollisionSound(myCollisions[0]->_penetration, deltaTime, VOXEL_COLLISION_FREQUENCY); } _trapDuration = isTrapped ? _trapDuration + deltaTime : 0.0f; + */ } void MyAvatar::applyHardCollision(const glm::vec3& penetration, float elasticity, float damping) { diff --git a/libraries/octree/src/Octree.cpp b/libraries/octree/src/Octree.cpp index 2af86663f7..ba76119fb7 100644 --- a/libraries/octree/src/Octree.cpp +++ b/libraries/octree/src/Octree.cpp @@ -751,6 +751,7 @@ bool findCapsulePenetrationOp(OctreeElement* element, void* extraData) { return false; } +/* TODO: Andrew to reimplement or purge this bool findShapeCollisionsOp(OctreeElement* element, void* extraData) { ShapeArgs* args = static_cast(extraData); @@ -771,6 +772,7 @@ bool findShapeCollisionsOp(OctreeElement* element, void* extraData) { } return false; } +*/ bool Octree::findCapsulePenetration(const glm::vec3& start, const glm::vec3& end, float radius, glm::vec3& penetration, Octree::lockType lockType, bool* accurateResult) { @@ -809,6 +811,7 @@ bool Octree::findCapsulePenetration(const glm::vec3& start, const glm::vec3& end return args.found; } +/* TODO: Andrew to reimplement or purge this bool Octree::findShapeCollisions(const Shape* shape, CollisionList& collisions, Octree::lockType lockType, bool* accurateResult) { @@ -839,6 +842,7 @@ bool Octree::findShapeCollisions(const Shape* shape, CollisionList& collisions, } return args.found; } +*/ class GetElementEnclosingArgs { public: diff --git a/libraries/octree/src/Octree.h b/libraries/octree/src/Octree.h index 7ab22598ef..978cffd724 100644 --- a/libraries/octree/src/Octree.h +++ b/libraries/octree/src/Octree.h @@ -277,8 +277,9 @@ public: bool findCapsulePenetration(const glm::vec3& start, const glm::vec3& end, float radius, glm::vec3& penetration, Octree::lockType lockType = Octree::TryLock, bool* accurateResult = NULL); - bool findShapeCollisions(const Shape* shape, CollisionList& collisions, - Octree::lockType = Octree::TryLock, bool* accurateResult = NULL); +// TODO: Andrew to reimplement or purge this +// bool findShapeCollisions(const Shape* shape, CollisionList& collisions, +// Octree::lockType = Octree::TryLock, bool* accurateResult = NULL); OctreeElement* getElementEnclosingPoint(const glm::vec3& point, Octree::lockType lockType = Octree::TryLock, bool* accurateResult = NULL); diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index f793163552..0a5d167ff7 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -9,8 +9,6 @@ // See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html // -#include - #include #include "ShapeCollider.h" @@ -127,29 +125,6 @@ bool collideShapesWithShapes(const QVector& shapesA, const QVectorgetType(); - if (typeA == SPHERE_SHAPE) { - return sphereVsAACube(static_cast(shapeA), cubeCenter, cubeSide, collisions); - } else if (typeA == CAPSULE_SHAPE) { - return capsuleVsAACube(static_cast(shapeA), cubeCenter, cubeSide, collisions); - } else if (typeA == LIST_SHAPE) { - const ListShape* listA = static_cast(shapeA); - bool touching = false; - for (int i = 0; i < listA->size() && !collisions.isFull(); ++i) { - const Shape* subShape = listA->getSubShape(i); - int subType = subShape->getType(); - if (subType == SPHERE_SHAPE) { - touching = sphereVsAACube(static_cast(subShape), cubeCenter, cubeSide, collisions) || touching; - } else if (subType == CAPSULE_SHAPE) { - touching = capsuleVsAACube(static_cast(subShape), cubeCenter, cubeSide, collisions) || touching; - } - } - return touching; - } - return false; -} - bool sphereVsSphere(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { const SphereShape* sphereA = static_cast(shapeA); const SphereShape* sphereB = static_cast(shapeB); @@ -536,28 +511,24 @@ bool planeVsPlane(const Shape* shapeA, const Shape* shapeB, CollisionList& colli return false; } -bool sphereVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { +// helper function +CollisionInfo* sphereVsAACubeHelper(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& cubeCenter, + float cubeSide, CollisionList& collisions) { + // sphere is A + // cube is B // BA = B - A = from center of A to center of B - const SphereShape* sphereA = static_cast(shapeA); - const AACubeShape* cubeB = static_cast(shapeB); - - float halfCubeSide = 0.5f * cubeB->getScale(); - float sphereRadius = sphereA->getRadius(); - - glm::vec3 sphereCenter = shapeA->getTranslation(); - glm::vec3 cubeCenter = shapeB->getTranslation(); + float halfCubeSide = 0.5f * cubeSide; glm::vec3 BA = cubeCenter - sphereCenter; - float distance = glm::length(BA); if (distance > EPSILON) { float maxBA = glm::max(glm::max(glm::abs(BA.x), glm::abs(BA.y)), glm::abs(BA.z)); if (maxBA > halfCubeSide + sphereRadius) { // sphere misses cube entirely - return false; + return NULL; } CollisionInfo* collision = collisions.getNewCollision(); if (!collision) { - return false; + return NULL; } if (maxBA > halfCubeSide) { // sphere hits cube but its center is outside cube @@ -590,7 +561,7 @@ bool sphereVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& col lengthDirection = glm::length(direction); } else if (lengthDirection > sphereRadius) { collisions.deleteLastCollision(); - return false; + return NULL; } direction /= lengthDirection; @@ -607,12 +578,14 @@ bool sphereVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& col direction /= lengthDirection; // compute collision details + collision->_floatData = cubeSide; + collision->_vecData = cubeCenter; collision->_penetration = (halfCubeSide * lengthDirection + sphereRadius - maxBA * glm::dot(BA, direction)) * direction; collision->_contactPoint = sphereCenter + sphereRadius * direction; } - collision->_shapeA = shapeA; - collision->_shapeB = shapeB; - return true; + collision->_shapeA = NULL; + collision->_shapeB = NULL; + return collision; } else if (sphereRadius + halfCubeSide > distance) { // NOTE: for cocentric approximation we collide sphere and cube as two spheres which means // this algorithm will probably be wrong when both sphere and cube are very small (both ~EPSILON) @@ -622,43 +595,145 @@ bool sphereVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& col collision->_penetration = (sphereRadius + halfCubeSide) * glm::vec3(0.0f, -1.0f, 0.0f); // contactPoint is on surface of A collision->_contactPoint = sphereCenter + collision->_penetration; - - collision->_shapeA = shapeA; - collision->_shapeB = shapeB; - return true; + collision->_shapeA = NULL; + collision->_shapeB = NULL; + return collision; } } + return NULL; +} + +bool sphereVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { + // BA = B - A = from center of A to center of B + const SphereShape* sphereA = static_cast(shapeA); + const AACubeShape* cubeB = static_cast(shapeB); + + CollisionInfo* collision = sphereVsAACubeHelper( sphereA->getTranslation(), sphereA->getRadius(), + cubeB->getTranslation(), cubeB->getScale(), collisions); + if (collision) { + collision->_shapeA = shapeA; + collision->_shapeB = shapeB; + return true; + } return false; } +glm::vec3 cubeNormals[3] = { glm::vec3(1.0f, 0.0f, 0.0f), glm::vec3(0.0f, 1.0f, 0.0f), glm::vec3(0.0f, 0.0f, 1.0f) }; + +// the wallIndices is a sequence of pairs that represent the OTHER directions for each cube face, +// hence the first pair is (1, 2) because the OTHER faces for xFace are (yFace, zFace) = (1, 2) +int wallIndices[] = { 1, 2, 0, 2, 0, 1 }; + bool capsuleVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - /* const CapsuleShape* capsuleA = static_cast(shapeA); const AACubeShape* cubeB = static_cast(shapeB); - // find nearest approach of capsule line segment to cube center - - // use nearest approach to find approximate point on cube surface - - // find point on cube surface closest to capsule line segment - - // collide like point inside sphere - - glm::vec3 capsuleAxis; + // find nearest approach of capsule's line segment to cube's center + glm::vec3 capsuleAxis; capsuleA->computeNormalizedAxis(capsuleAxis); - glm::vec3 cubeCenter = shapeB->getTranslation(); - float offset = glm::dot(cubeCenter - capsuleA->getTranslation(), capsuleAxis); float halfHeight = capsuleA->getHalfHeight(); - if (offset > halfHeight) { - offset = halfHeight; - } else if (offset < -halfHeight) { - offset = -halfHeight; + glm::vec3 cubeCenter = cubeB->getTranslation(); + glm::vec3 BA = cubeCenter - capsuleA->getTranslation(); + float axialOffset = glm::dot(capsuleAxis, BA); + if (fabsf(axialOffset) > halfHeight) { + axialOffset = (axialOffset < 0.0f) ? -halfHeight : halfHeight; + } + glm::vec3 nearestApproach = axialOffset * capsuleAxis; + + // transform nearestApproach into cube-relative frame + nearestApproach -= cubeCenter; + + // determine the face of nearest approach + glm::vec3 signs = glm::sign(nearestApproach); + int faceIndex = 0; + glm::vec3 faceNormal(signs.x, 0.0f, 0.0f); + float maxApproach = fabsf(nearestApproach.x); + if (maxApproach < fabsf(nearestApproach.y)) { + maxApproach = fabsf(nearestApproach.y); + faceIndex = 1; + faceNormal = glm::vec3(0.0f, signs.y, 0.0f); + } + if (maxApproach < fabsf(nearestApproach.z)) { + maxApproach = fabsf(nearestApproach.z); + faceIndex = 2; + faceNormal = glm::vec3(0.0f, 0.0f, signs.z); + } + + // Revisualize the capsule as a startPoint and an axis that points toward the cube face. + // We raytrace forward into the four planes that neigbhor the face to find the furthest + // point along the capsule's axis that might hit face. + glm::vec3 capsuleStart; + if (glm::dot(capsuleAxis, faceNormal) < 0.0f) { + capsuleA->getStartPoint(capsuleStart); + } else { + // NOTE: we want dot(capsuleAxis, faceNormal) to be negative which simplifies some + // logic below, so we pretend the end is the start thereby reversing its axis. + capsuleA->getEndPoint(capsuleStart); + capsuleAxis *= -1.0f; + } + // translate into cube-relative frame + capsuleStart -= cubeCenter; + float capsuleLength = 2.0f * halfHeight; + float halfCubeSide = 0.5f * cubeB->getScale(); + float capsuleRadius = capsuleA->getRadius(); + + // Loop over the directions that are NOT parallel to face (there are two of them). + // For each direction we'll raytrace along capsuleAxis to find point impact + // on the furthest face and clamp it to remain on the capsule's line segment + float distances[2] = { 0, capsuleLength}; + int numCompleteMisses = 0; + for (int i = 0; i < 2; ++i) { + int wallIndex = wallIndices[2 * faceIndex + i]; + // each direction has two walls: positive and negative + for (float wallSign = -1.0f; wallSign < 2.0f; wallSign += 2.0f) { + glm::vec3 wallNormal = wallSign * cubeNormals[wallIndex]; + float axisDotWall = glm::dot(capsuleAxis, wallNormal); + if (axisDotWall > EPSILON) { + float distance = (halfCubeSide + glm::dot(capsuleStart, wallNormal)) / axisDotWall; + distances[i] = glm::clamp(distance, 0.0f, capsuleLength); + if (distance == distances[i]) { + // the wall truncated the capsule which means there is a possibility that the capusule + // actually collides against the edge of the face, so we check for that case now + glm::vec3 impact = capsuleStart + distance * capsuleAxis; + float depth = glm::dot(impact, faceNormal) - halfCubeSide; + if (depth > 0.0f) { + if (depth < capsuleRadius) { + // need to recast against the diagonal plane: wall rotated away from the face + glm::vec3 diagonalNormal = INV_SQUARE_ROOT_OF_2 * (wallNormal - faceNormal); + distances[i] = glm::min(glm::dot(capsuleStart, diagonalNormal), 0.0f); + } else { + // capsule misses this wall by more than capsuleRadius + ++numCompleteMisses; + } + } + } + // there can't be more than one hit for any direction so we break + break; + } + } + } + if (numCompleteMisses == 2) { + return false; + } + + // chose the point that produces the deepest penetration against face + glm::vec3 point0 = capsuleStart + distances[0] * capsuleAxis; + glm::vec3 point1 = capsuleStart + distances[1] * capsuleAxis; + if (glm::dot(point0, faceNormal) > glm::dot(point1, faceNormal)) { + point0 = point1; + } + // move back into real frame + point0 += cubeCenter; + + // collide like a sphere at point0 with capsuleRadius + CollisionInfo* collision = sphereVsAACubeHelper(point0, capsuleRadius, + cubeCenter, 2.0f * halfCubeSide, collisions); + if (collision) { + // we hit! so store back pointers to the shapes + collision->_shapeA = shapeA; + collision->_shapeB = shapeB; + return true; } - glm::vec3 BA = cubeCenter - sphereCenter; - glm::vec3 nearestApproach = capsuleA->getTranslation() + offset * capsuleAxis; - // collide nearest approach like a sphere at that point - return sphereVsAACube(nearestApproach, capsuleA->getRadius(), cubeCenter, cubeSide, collisions); - */ return false; } @@ -707,103 +782,6 @@ bool listVsList(const Shape* shapeA, const Shape* shapeB, CollisionList& collisi return touching; } -// helper function -bool sphereVsAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& cubeCenter, - float cubeSide, CollisionList& collisions) { - // sphere is A - // cube is B - // BA = B - A = from center of A to center of B - float halfCubeSide = 0.5f * cubeSide; - glm::vec3 BA = cubeCenter - sphereCenter; - float distance = glm::length(BA); - if (distance > EPSILON) { - float maxBA = glm::max(glm::max(glm::abs(BA.x), glm::abs(BA.y)), glm::abs(BA.z)); - if (maxBA > halfCubeSide + sphereRadius) { - // sphere misses cube entirely - return false; - } - CollisionInfo* collision = collisions.getNewCollision(); - if (!collision) { - return false; - } - if (maxBA > halfCubeSide) { - // sphere hits cube but its center is outside cube - - // compute contact anti-pole on cube (in cube frame) - glm::vec3 cubeContact = glm::abs(BA); - if (cubeContact.x > halfCubeSide) { - cubeContact.x = halfCubeSide; - } - if (cubeContact.y > halfCubeSide) { - cubeContact.y = halfCubeSide; - } - if (cubeContact.z > halfCubeSide) { - cubeContact.z = halfCubeSide; - } - glm::vec3 signs = glm::sign(BA); - cubeContact.x *= signs.x; - cubeContact.y *= signs.y; - cubeContact.z *= signs.z; - - // compute penetration direction - glm::vec3 direction = BA - cubeContact; - float lengthDirection = glm::length(direction); - if (lengthDirection < EPSILON) { - // sphereCenter is touching cube surface, so we can't use the difference between those two - // points to compute the penetration direction. Instead we use the unitary components of - // cubeContact. - direction = cubeContact / halfCubeSide; - glm::modf(BA, direction); - lengthDirection = glm::length(direction); - } else if (lengthDirection > sphereRadius) { - collisions.deleteLastCollision(); - return false; - } - direction /= lengthDirection; - - // compute collision details - collision->_contactPoint = sphereCenter + sphereRadius * direction; - collision->_penetration = sphereRadius * direction - (BA - cubeContact); - } else { - // sphere center is inside cube - // --> push out nearest face - glm::vec3 direction; - BA /= maxBA; - glm::modf(BA, direction); - float lengthDirection = glm::length(direction); - direction /= lengthDirection; - - // compute collision details - collision->_floatData = cubeSide; - collision->_vecData = cubeCenter; - collision->_penetration = (halfCubeSide * lengthDirection + sphereRadius - maxBA * glm::dot(BA, direction)) * direction; - collision->_contactPoint = sphereCenter + sphereRadius * direction; - } - collision->_floatData = cubeSide; - collision->_vecData = cubeCenter; - collision->_shapeA = NULL; - collision->_shapeB = NULL; - return true; - } else if (sphereRadius + halfCubeSide > distance) { - // NOTE: for cocentric approximation we collide sphere and cube as two spheres which means - // this algorithm will probably be wrong when both sphere and cube are very small (both ~EPSILON) - CollisionInfo* collision = collisions.getNewCollision(); - if (collision) { - // the penetration and contactPoint are undefined, so we pick a penetration direction (-yAxis) - collision->_penetration = (sphereRadius + halfCubeSide) * glm::vec3(0.0f, -1.0f, 0.0f); - // contactPoint is on surface of A - collision->_contactPoint = sphereCenter + collision->_penetration; - - collision->_floatData = cubeSide; - collision->_vecData = cubeCenter; - collision->_shapeA = NULL; - collision->_shapeB = NULL; - return true; - } - } - return false; -} - // helper function /* KEEP THIS CODE -- this is how to collide the cube with stark face normals (no rounding). * We might want to use this code later for sealing boundaries between adjacent voxels. @@ -856,26 +834,6 @@ bool sphereAACube_StarkAngles(const glm::vec3& sphereCenter, float sphereRadius, } */ -bool sphereVsAACube(const SphereShape* sphereA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) { - return sphereVsAACube(sphereA->getTranslation(), sphereA->getRadius(), cubeCenter, cubeSide, collisions); -} - -bool capsuleVsAACube(const CapsuleShape* capsuleA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) { - // find nearest approach of capsule line segment to cube - glm::vec3 capsuleAxis; - capsuleA->computeNormalizedAxis(capsuleAxis); - float offset = glm::dot(cubeCenter - capsuleA->getTranslation(), capsuleAxis); - float halfHeight = capsuleA->getHalfHeight(); - if (offset > halfHeight) { - offset = halfHeight; - } else if (offset < -halfHeight) { - offset = -halfHeight; - } - glm::vec3 nearestApproach = capsuleA->getTranslation() + offset * capsuleAxis; - // collide nearest approach like a sphere at that point - return sphereVsAACube(nearestApproach, capsuleA->getRadius(), cubeCenter, cubeSide, collisions); -} - bool findRayIntersectionWithShapes(const QVector shapes, const glm::vec3& rayStart, const glm::vec3& rayDirection, float& minDistance) { float hitDistance = FLT_MAX; int numShapes = shapes.size(); diff --git a/libraries/shared/src/ShapeCollider.h b/libraries/shared/src/ShapeCollider.h index 261b0640db..41008b51e7 100644 --- a/libraries/shared/src/ShapeCollider.h +++ b/libraries/shared/src/ShapeCollider.h @@ -39,13 +39,6 @@ namespace ShapeCollider { bool collideShapeWithShapes(const Shape* shapeA, const QVector& shapes, int startIndex, CollisionList& collisions); bool collideShapesWithShapes(const QVector& shapesA, const QVector& shapesB, CollisionList& collisions); - /// \param shapeA a pointer to a shape (cannot be NULL) - /// \param cubeCenter center of cube - /// \param cubeSide lenght of side of cube - /// \param collisions[out] average collision details - /// \return true if shapeA collides with axis aligned cube - bool collideShapeWithAACube(const Shape* shapeA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions); - /// \param sphereA pointer to first shape (cannot be NULL) /// \param sphereB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details @@ -100,6 +93,16 @@ namespace ShapeCollider { /// \return true if shapes collide bool planeVsPlane(const Shape* planeA, const Shape* planeB, CollisionList& collisions); + /// helper function for *VsAACube() methods + /// \param sphereCenter center of sphere + /// \param sphereRadius radius of sphere + /// \param cubeCenter center of AACube + /// \param cubeSide scale of cube + /// \param[out] collisions where to append collision details + /// \return valid pointer to CollisionInfo if sphere and cube overlap or NULL if not + CollisionInfo* sphereVsAACubeHelper(const glm::vec3& sphereCenter, float sphereRadius, + const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions); + bool sphereVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions); bool capsuleVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions); bool aaCubeVsSphere(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions); @@ -124,20 +127,6 @@ namespace ShapeCollider { /// \return true if shapes collide bool listVsList(const Shape* listA, const Shape* listB, CollisionList& collisions); - /// \param sphereA pointer to sphere (cannot be NULL) - /// \param cubeCenter center of cube - /// \param cubeSide lenght of side of cube - /// \param[out] collisions where to append collision details - /// \return true if sphereA collides with axis aligned cube - bool sphereVsAACube(const SphereShape* sphereA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions); - - /// \param capsuleA pointer to capsule (cannot be NULL) - /// \param cubeCenter center of cube - /// \param cubeSide lenght of side of cube - /// \param[out] collisions where to append collision details - /// \return true if capsuleA collides with axis aligned cube - bool capsuleVsAACube(const CapsuleShape* capsuleA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions); - /// \param shapes list of pointers to shapes (shape pointers may be NULL) /// \param startPoint beginning of ray /// \param direction direction of ray diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp index 45d3ed6508..d6b1b9d751 100644 --- a/tests/physics/src/ShapeColliderTests.cpp +++ b/tests/physics/src/ShapeColliderTests.cpp @@ -683,81 +683,93 @@ void ShapeColliderTests::capsuleTouchesCapsule() { void ShapeColliderTests::sphereTouchesAACubeFaces() { CollisionList collisions(16); - glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f); - float cubeSide = 2.34f; - float sphereRadius = 1.13f; glm::vec3 sphereCenter(0.0f); - SphereShape sphere(sphereRadius, sphereCenter); - QVector axes; - axes.push_back(xAxis); - axes.push_back(-xAxis); - axes.push_back(yAxis); - axes.push_back(-yAxis); - axes.push_back(zAxis); - axes.push_back(-zAxis); + glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f); + float cubeSide = 4.34f; - for (int i = 0; i < axes.size(); ++i) { - glm::vec3 axis = axes[i]; - // outside - { - collisions.clear(); - float overlap = 0.25f; - float sphereOffset = 0.5f * cubeSide + sphereRadius - overlap; - sphereCenter = cubeCenter + sphereOffset * axis; - sphere.setTranslation(sphereCenter); - - if (!ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube. axis = " << axis - << std::endl; - } - CollisionInfo* collision = collisions[0]; - if (!collision) { - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: no CollisionInfo. axis = " << axis << std::endl; - } - - glm::vec3 expectedPenetration = - overlap * axis; - if (glm::distance(expectedPenetration, collision->_penetration) > EPSILON) { - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: penetration = " << collision->_penetration - << " expected " << expectedPenetration << " axis = " << axis << std::endl; - } - - glm::vec3 expectedContact = sphereCenter - sphereRadius * axis; - if (glm::distance(expectedContact, collision->_contactPoint) > EPSILON) { - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: contactaPoint = " << collision->_contactPoint - << " expected " << expectedContact << " axis = " << axis << std::endl; - } - } + glm::vec3 faceNormals[] = {xAxis, yAxis, zAxis}; + int numDirections = 3; - // inside - { - collisions.clear(); - float overlap = 1.25f * sphereRadius; - float sphereOffset = 0.5f * cubeSide + sphereRadius - overlap; - sphereCenter = cubeCenter + sphereOffset * axis; - sphere.setTranslation(sphereCenter); + 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; - if (!ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube." - << " axis = " << axis << std::endl; - } - CollisionInfo* collision = collisions[0]; - if (!collision) { - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: no CollisionInfo on y-axis." - << " axis = " << axis << std::endl; + // 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) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube. faceNormal = " << faceNormal + << std::endl; + break; + } + + if (glm::distance(expectedPenetration, collision->_penetration) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: penetration = " << collision->_penetration + << " expected " << expectedPenetration << " faceNormal = " << faceNormal << std::endl; + } + + glm::vec3 expectedContact = sphereCenter - sphereRadius * faceNormal; + if (glm::distance(expectedContact, collision->_contactPoint) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: contactaPoint = " << collision->_contactPoint + << " expected " << expectedContact << " faceNormal = " << faceNormal << std::endl; + } + + if (collision->getShapeA()) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: collision->_shapeA should be NULL" << std::endl; + } + if (collision->getShapeB()) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: collision->_shapeB should be NULL" << std::endl; + } + } } - glm::vec3 expectedPenetration = - overlap * axis; - if (glm::distance(expectedPenetration, collision->_penetration) > EPSILON) { - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: penetration = " << collision->_penetration - << " expected " << expectedPenetration << " axis = " << axis << std::endl; - } + // 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); - glm::vec3 expectedContact = sphereCenter - sphereRadius * axis; - if (glm::distance(expectedContact, collision->_contactPoint) > EPSILON) { - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: contactaPoint = " << collision->_contactPoint - << " expected " << expectedContact << " axis = " << axis << std::endl; + if (!collision) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube." + << " faceNormal = " << faceNormal << std::endl; + break; + } + + glm::vec3 expectedPenetration = - overlap * faceNormal; + if (glm::distance(expectedPenetration, collision->_penetration) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: penetration = " << collision->_penetration + << " expected " << expectedPenetration << " faceNormal = " << faceNormal << std::endl; + } + + glm::vec3 expectedContact = sphereCenter - sphereRadius * faceNormal; + if (glm::distance(expectedContact, collision->_contactPoint) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: contactaPoint = " << collision->_contactPoint + << " expected " << expectedContact << " faceNormal = " << faceNormal << std::endl; + } } } } @@ -766,64 +778,133 @@ void ShapeColliderTests::sphereTouchesAACubeFaces() { void ShapeColliderTests::sphereTouchesAACubeEdges() { CollisionList collisions(20); - glm::vec3 cubeCenter(0.0f, 0.0f, 0.0f); - float cubeSide = 2.0f; - - float sphereRadius = 1.0f; + float sphereRadius = 1.37f; glm::vec3 sphereCenter(0.0f); - SphereShape sphere(sphereRadius, sphereCenter); - QVector axes; - // edges - axes.push_back(glm::vec3(0.0f, 1.0f, 1.0f)); - axes.push_back(glm::vec3(0.0f, 1.0f, -1.0f)); - axes.push_back(glm::vec3(0.0f, -1.0f, 1.0f)); - axes.push_back(glm::vec3(0.0f, -1.0f, -1.0f)); - axes.push_back(glm::vec3(1.0f, 1.0f, 0.0f)); - axes.push_back(glm::vec3(1.0f, -1.0f, 0.0f)); - axes.push_back(glm::vec3(-1.0f, 1.0f, 0.0f)); - axes.push_back(glm::vec3(-1.0f, -1.0f, 0.0f)); - axes.push_back(glm::vec3(1.0f, 0.0f, 1.0f)); - axes.push_back(glm::vec3(1.0f, 0.0f, -1.0f)); - axes.push_back(glm::vec3(-1.0f, 0.0f, 1.0f)); - axes.push_back(glm::vec3(-1.0f, 0.0f, -1.0f)); - // and corners - axes.push_back(glm::vec3(1.0f, 1.0f, 1.0f)); - axes.push_back(glm::vec3(1.0f, 1.0f, -1.0f)); - axes.push_back(glm::vec3(1.0f, -1.0f, 1.0f)); - axes.push_back(glm::vec3(1.0f, -1.0f, -1.0f)); - axes.push_back(glm::vec3(-1.0f, 1.0f, 1.0f)); - axes.push_back(glm::vec3(-1.0f, 1.0f, -1.0f)); - axes.push_back(glm::vec3(-1.0f, -1.0f, 1.0f)); - axes.push_back(glm::vec3(-1.0f, -1.0f, -1.0f)); + glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f); + float cubeSide = 2.98f; - for (int i =0; i < axes.size(); ++i) { - glm::vec3 axis = axes[i]; - float lengthAxis = glm::length(axis); - axis /= lengthAxis; - float overlap = 0.25f; - - sphereCenter = cubeCenter + (lengthAxis * 0.5f * cubeSide + sphereRadius - overlap) * axis; - sphere.setTranslation(sphereCenter); - - if (!ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube. axis = " << axis << std::endl; - } - CollisionInfo* collision = collisions[i]; - if (!collision) { - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: no CollisionInfo. axis = " << axis << std::endl; + float overlap = 0.25 * 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) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube edge." + << " edgeNormal = " << edgeNormal << std::endl; + break; + } + + if (glm::distance(expectedPenetration, collision->_penetration) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: penetration = " << collision->_penetration + << " expected " << expectedPenetration << " edgeNormal = " << edgeNormal << std::endl; + } + + glm::vec3 expectedContact = sphereCenter - sphereRadius * edgeNormal; + if (glm::distance(expectedContact, collision->_contactPoint) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: contactaPoint = " << collision->_contactPoint + << " expected " << expectedContact << " edgeNormal = " << edgeNormal << std::endl; + } + } + } + } } + } +} + +void ShapeColliderTests::sphereTouchesAACubeCorners() { + CollisionList collisions(20); - glm::vec3 expectedPenetration = - overlap * axis; - if (glm::distance(expectedPenetration, collision->_penetration) > EPSILON) { - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: penetration = " << collision->_penetration - << " expected " << expectedPenetration << " axis = " << axis << std::endl; - } + 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.25 * 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); - glm::vec3 expectedContact = sphereCenter - sphereRadius * axis; - if (glm::distance(expectedContact, collision->_contactPoint) > EPSILON) { - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: contactaPoint = " << collision->_contactPoint - << " expected " << expectedContact << " axis = " << axis << std::endl; + if (!collision) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube corner." + << " cornerNormal = " << cornerNormal << std::endl; + break; + } + + glm::vec3 expectedPenetration = - overlap * offsetAxis; + if (glm::distance(expectedPenetration, collision->_penetration) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: penetration = " << collision->_penetration + << " expected " << expectedPenetration << " cornerNormal = " << cornerNormal << std::endl; + } + + glm::vec3 expectedContact = sphereCenter - sphereRadius * offsetAxis; + if (glm::distance(expectedContact, collision->_contactPoint) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: contactaPoint = " << collision->_contactPoint + << " expected " << expectedContact << " cornerNormal = " << cornerNormal << std::endl; + } + } + } } } } @@ -831,55 +912,113 @@ void ShapeColliderTests::sphereTouchesAACubeEdges() { void ShapeColliderTests::sphereMissesAACube() { CollisionList collisions(16); + float sphereRadius = 1.0f; + glm::vec3 sphereCenter(0.0f); + glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f); float cubeSide = 2.0f; - float sphereRadius = 1.0f; - glm::vec3 sphereCenter(0.0f); - SphereShape sphere(sphereRadius, sphereCenter); + glm::vec3 faceNormals[] = {xAxis, yAxis, zAxis}; + int numDirections = 3; - float sphereOffset = (0.5f * cubeSide + sphereRadius + 0.25f); + float offset = 2.0f * EPSILON; - // top - sphereCenter = cubeCenter + sphereOffset * yAxis; - sphere.setTranslation(sphereCenter); - if (ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube" << std::endl; + // 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) { + 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); - // bottom - sphereCenter = cubeCenter - sphereOffset * yAxis; - sphere.setTranslation(sphereCenter); - if (ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube" << std::endl; + if (collision) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube edge." + << " edgeNormal = " << edgeNormal << std::endl; + } + } + + } + } + } } - // left - sphereCenter = cubeCenter + sphereOffset * xAxis; - sphere.setTranslation(sphereCenter); - if (ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube" << std::endl; - } + // 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]; - // right - sphereCenter = cubeCenter - sphereOffset * xAxis; - sphere.setTranslation(sphereCenter); - if (ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube" << std::endl; - } + // the cornerNormal is the normalized sum of the three faces + glm::vec3 cornerNormal = glm::normalize(firstNormal + secondNormal + thirdNormal); - // forward - sphereCenter = cubeCenter + sphereOffset * zAxis; - sphere.setTranslation(sphereCenter); - if (ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube" << std::endl; - } + // 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); - // back - sphereCenter = cubeCenter - sphereOffset * zAxis; - sphere.setTranslation(sphereCenter); - if (ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube" << std::endl; + // 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) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube corner." + << " cornerNormal = " << cornerNormal << std::endl; + break; + } + } + } + } } } @@ -1347,6 +1486,7 @@ void ShapeColliderTests::runAllTests() { sphereTouchesAACubeFaces(); sphereTouchesAACubeEdges(); + sphereTouchesAACubeCorners(); sphereMissesAACube(); rayHitsSphere(); diff --git a/tests/physics/src/ShapeColliderTests.h b/tests/physics/src/ShapeColliderTests.h index 4a51651cb8..16e0e0c409 100644 --- a/tests/physics/src/ShapeColliderTests.h +++ b/tests/physics/src/ShapeColliderTests.h @@ -25,6 +25,7 @@ namespace ShapeColliderTests { void sphereTouchesAACubeFaces(); void sphereTouchesAACubeEdges(); + void sphereTouchesAACubeCorners(); void sphereMissesAACube(); void rayHitsSphere();