From c8e56a97cc90777265c320fa7e1a2fc8c9817435 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 2 May 2014 21:47:27 -0700 Subject: [PATCH] fix sphere-cube collisions, with tests --- libraries/shared/src/ShapeCollider.cpp | 125 ++++++++++----- tests/physics/src/ShapeColliderTests.cpp | 191 ++++++++++++++++++----- tests/physics/src/ShapeColliderTests.h | 3 +- 3 files changed, 241 insertions(+), 78 deletions(-) diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index cc4093d2b1..ceba1b4c5e 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -15,7 +15,6 @@ #include "GeometryUtil.h" #include "ShapeCollider.h" -#include "StreamUtils.h" // adebug // NOTE: // @@ -594,6 +593,95 @@ bool listList(const ListShape* listA, const ListShape* listB, CollisionList& col // helper function bool sphereAACube(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 + + // contactPoint is on surface of sphere + glm::vec3 cubeContact = glm::abs(BA); + glm::vec3 direction = cubeContact - glm::vec3(halfCubeSide); + + if (direction.x < 0.0f) { + direction.x = 0.0f; + } + if (direction.y < 0.0f) { + direction.y = 0.0f; + } + if (direction.z < 0.0f) { + direction.z = 0.0f; + } + + glm::vec3 signs = glm::sign(BA); + direction.x *= signs.x; + direction.y *= signs.y; + direction.z *= signs.z; + direction = glm::normalize(direction); + collision->_contactPoint = sphereCenter + sphereRadius * direction; + + // penetration points from contact point on cube to that on sphere + if (cubeContact.x > halfCubeSide) { + cubeContact.x = halfCubeSide; + } + cubeContact.x *= -signs.x; + if (cubeContact.y > halfCubeSide) { + cubeContact.y = halfCubeSide; + } + cubeContact.y *= -signs.y; + if (cubeContact.z > halfCubeSide) { + cubeContact.z = halfCubeSide; + } + cubeContact.z *= -signs.z; + //collision->_penetration = collision->_contactPoint - cubeCenter + cubeContact; + collision->_penetration = collision->_contactPoint - (cubeCenter + cubeContact); + } else { + // sphere center is inside cube + // --> push out nearest face + glm::vec3 direction; + BA /= distance; + glm::modf(BA, direction); + direction = glm::normalize(direction); + + // penetration is the projection of surfaceAB on direction + collision->_penetration = (halfCubeSide + sphereRadius - distance * glm::dot(BA, direction)) * direction; + // contactPoint is on surface of A + collision->_contactPoint = sphereCenter + sphereRadius * direction; + } + 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; + 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. +bool sphereAACube_StarkAngles(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& cubeCenter, + float cubeSide, CollisionList& collisions) { glm::vec3 BA = cubeCenter - sphereCenter; float distance = glm::length(BA); if (distance > EPSILON) { @@ -608,45 +696,11 @@ bool sphereAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm:: if (glm::dot(surfaceAB, BA) > 0.f) { CollisionInfo* collision = collisions.getNewCollision(); if (collision) { - /* 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. // penetration is parallel to box side direction BA /= maxBA; glm::vec3 direction; glm::modf(BA, direction); direction = glm::normalize(direction); - */ - - // For rounded normals at edges and corners: - // At this point imagine that sphereCenter touches a "normalized" cube with rounded edges. - // This cube has a sidelength of 2 and its smoothing radius is sphereRadius/maxBA. - // We're going to try to compute the "negative normal" (and hence direction of penetration) - // of this surface. - - float radius = sphereRadius / (distance * maxBA); // normalized radius - float shortLength = maxBA - radius; - glm::vec3 direction = BA; - if (shortLength > 0.0f) { - direction = glm::abs(BA) - glm::vec3(shortLength); - // Set any negative components to zero, and adopt the sign of the original BA component. - // Unfortunately there isn't an easy way to make this fast. - if (direction.x < 0.0f) { - direction.x = 0.f; - } else if (BA.x < 0.f) { - direction.x = -direction.x; - } - if (direction.y < 0.0f) { - direction.y = 0.f; - } else if (BA.y < 0.f) { - direction.y = -direction.y; - } - if (direction.z < 0.0f) { - direction.z = 0.f; - } else if (BA.z < 0.f) { - direction.z = -direction.z; - } - } - direction = glm::normalize(direction); // penetration is the projection of surfaceAB on direction collision->_penetration = glm::dot(surfaceAB, direction) * direction; @@ -669,6 +723,7 @@ bool sphereAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm:: } return false; } +*/ bool sphereAACube(const SphereShape* sphereA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) { return sphereAACube(sphereA->getPosition(), sphereA->getRadius(), cubeCenter, cubeSide, collisions); diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp index 3f952236e2..7b3d956065 100644 --- a/tests/physics/src/ShapeColliderTests.cpp +++ b/tests/physics/src/ShapeColliderTests.cpp @@ -681,58 +681,164 @@ void ShapeColliderTests::capsuleTouchesCapsule() { } } -void ShapeColliderTests::sphereTouchesAACube() { +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); + + 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.setPosition(sphereCenter); + + if (!ShapeCollider::sphereAACube(&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; + } + } + + // inside + { + collisions.clear(); + float overlap = 1.25f * sphereRadius; + float sphereOffset = 0.5f * cubeSide + sphereRadius - overlap; + sphereCenter = cubeCenter + sphereOffset * axis; + sphere.setPosition(sphereCenter); + + if (!ShapeCollider::sphereAACube(&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; + } + + 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; + } + } + } +} + +void ShapeColliderTests::sphereTouchesAACubeEdges() { + CollisionList collisions(20); + + glm::vec3 cubeCenter(0.0f, 0.0f, 0.0f); float cubeSide = 2.0f; float sphereRadius = 1.0f; glm::vec3 sphereCenter(0.0f); SphereShape sphere(sphereRadius, sphereCenter); - float sphereOffset = (0.5f * cubeSide + sphereRadius - 0.25f); + 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)); - // top - sphereCenter = cubeCenter + sphereOffset * yAxis; - sphere.setPosition(sphereCenter); - if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl; - } + for (int i =0; i < axes.size(); ++i) { + glm::vec3 axis = axes[i]; + float lengthAxis = glm::length(axis); + axis /= lengthAxis; + float overlap = 0.25f; - // bottom - sphereCenter = cubeCenter - sphereOffset * yAxis; - sphere.setPosition(sphereCenter); - if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl; - } - - // left - sphereCenter = cubeCenter + sphereOffset * xAxis; - sphere.setPosition(sphereCenter); - if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl; - } - - // right - sphereCenter = cubeCenter - sphereOffset * xAxis; - sphere.setPosition(sphereCenter); - if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl; - } - - // forward - sphereCenter = cubeCenter + sphereOffset * zAxis; - sphere.setPosition(sphereCenter); - if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl; - } - - // back - sphereCenter = cubeCenter - sphereOffset * zAxis; - sphere.setPosition(sphereCenter); - if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl; + sphereCenter = cubeCenter + (lengthAxis * 0.5f * cubeSide + sphereRadius - overlap) * axis; + sphere.setPosition(sphereCenter); + + if (!ShapeCollider::sphereAACube(&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; + } + + 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; + } } } @@ -802,6 +908,7 @@ void ShapeColliderTests::runAllTests() { capsuleMissesCapsule(); capsuleTouchesCapsule(); - sphereTouchesAACube(); + sphereTouchesAACubeFaces(); + sphereTouchesAACubeEdges(); sphereMissesAACube(); } diff --git a/tests/physics/src/ShapeColliderTests.h b/tests/physics/src/ShapeColliderTests.h index a94f5050ff..b51c48a61e 100644 --- a/tests/physics/src/ShapeColliderTests.h +++ b/tests/physics/src/ShapeColliderTests.h @@ -23,7 +23,8 @@ namespace ShapeColliderTests { void capsuleMissesCapsule(); void capsuleTouchesCapsule(); - void sphereTouchesAACube(); + void sphereTouchesAACubeFaces(); + void sphereTouchesAACubeEdges(); void sphereMissesAACube(); void runAllTests();