From 00913d4422318d22d060f602ea3bbc92b12b94a2 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 28 Aug 2014 16:51:16 -0700 Subject: [PATCH] fixes for capsuleVsAACube() with unit tests --- libraries/shared/src/ShapeCollider.cpp | 84 ++-- tests/physics/src/ShapeColliderTests.cpp | 555 +++++++++++++++++++++++ tests/physics/src/ShapeColliderTests.h | 3 + 3 files changed, 605 insertions(+), 37 deletions(-) diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index e4e769b1c7..7aaf71c44d 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -624,8 +624,6 @@ glm::vec3 cubeNormals[3] = { glm::vec3(1.0f, 0.0f, 0.0f), glm::vec3(0.0f, 1.0f, // 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 }; -const float INV_SQRT_TWO = 1.0f / SQUARE_ROOT_OF_2; - bool capsuleVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { const CapsuleShape* capsuleA = static_cast(shapeA); const AACubeShape* cubeB = static_cast(shapeB); @@ -635,12 +633,13 @@ bool capsuleVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& co capsuleA->computeNormalizedAxis(capsuleAxis); float halfHeight = capsuleA->getHalfHeight(); glm::vec3 cubeCenter = cubeB->getTranslation(); - glm::vec3 BA = cubeCenter - capsuleA->getTranslation(); + glm::vec3 capsuleCenter = capsuleA->getTranslation(); + glm::vec3 BA = cubeCenter - capsuleCenter; float axialOffset = glm::dot(capsuleAxis, BA); if (fabsf(axialOffset) > halfHeight) { axialOffset = (axialOffset < 0.0f) ? -halfHeight : halfHeight; } - glm::vec3 nearestApproach = axialOffset * capsuleAxis; + glm::vec3 nearestApproach = capsuleCenter + axialOffset * capsuleAxis; // transform nearestApproach into cube-relative frame nearestApproach -= cubeCenter; @@ -661,6 +660,13 @@ bool capsuleVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& co faceNormal = glm::vec3(0.0f, 0.0f, signs.z); } + if (fabs(glm::dot(faceNormal, capsuleAxis)) < EPSILON) { + // TODO: Andrew to implement this special case + // where capsule is perpendicular to the face that it hits + // (Make this its own function? or implement it here?) + return false; + } + // 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. @@ -680,10 +686,10 @@ bool capsuleVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& co 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 each direction we'll raytrace along capsuleAxis to find where the axis passes + // through the furthest face and then we'll clamp to remain on the capsule's line segment + float shortestDistance = capsuleLength; + for (int i = 0; i < 2; ++i) { int wallIndex = wallIndices[2 * faceIndex + i]; // each direction has two walls: positive and negative @@ -691,44 +697,48 @@ bool capsuleVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& co 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_SQRT_TWO * (wallNormal - faceNormal); - distances[i] = glm::min(glm::dot(capsuleStart, diagonalNormal), 0.0f); - } else { - // capsule misses this wall by more than capsuleRadius - ++numCompleteMisses; - } + // formula for distance to intersection between line (P,p) and plane (V,n) is: (V-P)*n / p*n + float newDistance = (halfCubeSide - glm::dot(capsuleStart, wallNormal)) / axisDotWall; + if (newDistance < 0.0f) { + // The wall is behind the capsule, but there is still a possibility that it collides + // against the edge so we recast against the diagonal plane beteween the two faces. + // NOTE: it is impossible for the startPoint to be in front of the diagonal plane, + // therefore we know we'll get a valid distance. + glm::vec3 thirdNormal = glm::cross(faceNormal, wallNormal); + glm::vec3 diagonalNormal = glm::normalize(glm::cross(glm::cross(capsuleAxis, thirdNormal), thirdNormal)); + newDistance = glm::dot(halfCubeSide * (faceNormal + wallNormal) - capsuleStart, diagonalNormal) / + glm::dot(capsuleAxis, diagonalNormal); + } else if (newDistance < capsuleLength) { + // The wall truncates the capsule axis, but we must check the case where the capsule + // collides with an edge/corner rather than the face. The good news is that this gives us + // an opportunity to check for an early exit case. + float heightOfImpact = glm::dot(capsuleStart + newDistance * capsuleAxis, faceNormal); + if (heightOfImpact > halfCubeSide + SQUARE_ROOT_OF_2 * capsuleRadius) { + // it is impossible for the capsule to hit the face + return false; + } else { + // recast against the diagonal plane between the two faces + glm::vec3 thirdNormal = glm::cross(faceNormal, wallNormal); + glm::vec3 diagonalNormal = glm::normalize(glm::cross(glm::cross(capsuleAxis, thirdNormal), thirdNormal)); + newDistance = glm::dot(halfCubeSide * (faceNormal + wallNormal) - capsuleStart, diagonalNormal) / + glm::dot(capsuleAxis, diagonalNormal); } } - // there can't be more than one hit for any direction so we break + if (newDistance < shortestDistance) { + shortestDistance = newDistance; + } + // there can only be one hit per direction 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; + // and translate back into real frame + glm::vec3 sphereCenter = cubeCenter + capsuleStart + shortestDistance * capsuleAxis; // collide like a sphere at point0 with capsuleRadius - CollisionInfo* collision = sphereVsAACubeHelper(point0, capsuleRadius, + CollisionInfo* collision = sphereVsAACubeHelper(sphereCenter, capsuleRadius, cubeCenter, 2.0f * halfCubeSide, collisions); if (collision) { // we hit! so store back pointers to the shapes diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp index 0ae6ef2f47..ab9845ed8b 100644 --- a/tests/physics/src/ShapeColliderTests.cpp +++ b/tests/physics/src/ShapeColliderTests.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include #include @@ -1022,6 +1023,557 @@ void ShapeColliderTests::sphereTouchesAACubeCorners() { } } +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; + + // 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 + bool hit = ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions); + if (hit) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should NOT collide with cube face." + << " faceNormal = " << faceNormal << std::endl; + } + } + } + + // 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 + bool hit = ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions); + if (hit) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should NOT collide with cube face." + << " edgeNormal = " << edgeNormal << 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]; + + // 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 + bool hit = ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions); + if (hit) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should NOT collide with cube face." + << " cornerNormal = " << cornerNormal << std::endl; + } + } + } + } +} + +void ShapeColliderTests::capsuleTouchesAACube() { + CollisionList collisions(16); + + float capsuleRadius = 1.0f; + + //glm::vec3 cubeCenter(0.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 against 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 + bool hit = ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions); + if (!hit) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should collide with cube" + << " faceNormal = " << faceNormal << std::endl; + break; + } + + CollisionInfo* collision = collisions.getLastCollision(); + if (!collision) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: null collision with faceNormal = " << faceNormal << std::endl; + return; + } + + // penetration points from capsule into cube + glm::vec3 expectedPenetration = - overlap * faceNormal; + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); + if (fabs(inaccuracy) > allowableError) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration + << " faceNormal = " << faceNormal + << std::endl; + } + + // contactPoint is on surface of capsule + glm::vec3 expectedContactPoint = collidingPoint - capsuleRadius * faceNormal; + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > allowableError) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint + << " faceNormal = " << faceNormal + << std::endl; + } + } + } + + // capsule caps against 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 + bool hit = ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions); + if (!hit) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should collide with cube" + << " edgeNormal = " << edgeNormal << std::endl; + } + + CollisionInfo* collision = collisions.getLastCollision(); + if (!collision) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: null collision with edgeNormal = " << edgeNormal << std::endl; + return; + } + + // penetration points from capsule into cube + glm::vec3 expectedPenetration = - overlap * edgeNormal; + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); + if (fabs(inaccuracy) > allowableError) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration + << " edgeNormal = " << edgeNormal + << std::endl; + } + + // contactPoint is on surface of capsule + glm::vec3 expectedContactPoint = collidingPoint - capsuleRadius * edgeNormal; + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > allowableError) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint + << " edgeNormal = " << edgeNormal + << std::endl; + } + } + } + } + } + + // capsule caps against 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 + bool hit = ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions); + if (!hit) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should collide with cube" + << " cornerNormal = " << cornerNormal << std::endl; + } + + CollisionInfo* collision = collisions.getLastCollision(); + if (!collision) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: null collision with cornerNormal = " << cornerNormal << std::endl; + return; + } + + // penetration points from capsule into cube + glm::vec3 expectedPenetration = - overlap * cornerNormal; + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); + if (fabs(inaccuracy) > allowableError) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration + << " cornerNormal = " << cornerNormal + << std::endl; + } + + // contactPoint is on surface of capsule + glm::vec3 expectedContactPoint = collidingPoint - capsuleRadius * cornerNormal; + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > allowableError) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint + << " cornerNormal = " << cornerNormal + << std::endl; + } + } + } + } + + // capsule sides against 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 + bool hit = ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions); + if (!hit) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should collide with cube" + << " edgeNormal = " << edgeNormal << std::endl; + } + + CollisionInfo* collision = collisions.getLastCollision(); + if (!collision) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: null collision with edgeNormal = " << edgeNormal << std::endl; + return; + } + + // penetration points from capsule into cube + glm::vec3 expectedPenetration = - overlap * deflectedNormal; + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); + if (fabs(inaccuracy) > allowableError / capsuleLength) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration + << " edgeNormal = " << edgeNormal + << std::endl; + } + + // contactPoint is on surface of capsule + glm::vec3 expectedContactPoint = axisPoint - capsuleRadius * deflectedNormal; + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > allowableError / capsuleLength) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint + << " edgeNormal = " << edgeNormal + << std::endl; + } + } + } + } + } + + // capsule sides against 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 + bool hit = ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions); + if (!hit) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should collide with cube" + << " cornerNormal = " << cornerNormal << std::endl; + } + + CollisionInfo* collision = collisions.getLastCollision(); + if (!collision) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: null collision with cornerNormal = " << cornerNormal << std::endl; + return; + } + + // penetration points from capsule into cube + glm::vec3 expectedPenetration = overlap * penetrationNormal; + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); + if (fabs(inaccuracy) > allowableError) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration + << " cornerNormal = " << cornerNormal + << std::endl; + } + + // contactPoint is on surface of capsule + glm::vec3 expectedContactPoint = collidingPoint + capsuleRadius * penetrationNormal; + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > allowableError) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint + << " cornerNormal = " << cornerNormal + << std::endl; + } + } + } + } +} + + void ShapeColliderTests::rayHitsSphere() { float startDistance = 3.0f; glm::vec3 rayStart(-startDistance, 0.0f, 0.0f); @@ -1489,6 +2041,9 @@ void ShapeColliderTests::runAllTests() { sphereTouchesAACubeEdges(); sphereTouchesAACubeCorners(); + capsuleMissesAACube(); + capsuleTouchesAACube(); + rayHitsSphere(); rayBarelyHitsSphere(); rayBarelyMissesSphere(); diff --git a/tests/physics/src/ShapeColliderTests.h b/tests/physics/src/ShapeColliderTests.h index 16e0e0c409..a7495d32bf 100644 --- a/tests/physics/src/ShapeColliderTests.h +++ b/tests/physics/src/ShapeColliderTests.h @@ -28,6 +28,9 @@ namespace ShapeColliderTests { void sphereTouchesAACubeCorners(); void sphereMissesAACube(); + void capsuleMissesAACube(); + void capsuleTouchesAACube(); + void rayHitsSphere(); void rayBarelyHitsSphere(); void rayBarelyMissesSphere();