diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 10ae4b0303..9017867399 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -3349,8 +3349,6 @@ void Application::updateLocationInServer() { if (accountManager.isLoggedIn()) { - static QJsonObject lastLocationObject; - // construct a QJsonObject given the user's current address information QJsonObject updatedLocationObject; @@ -3361,14 +3359,9 @@ void Application::updateLocationInServer() { updatedLocationObject.insert("address", addressObject); - if (updatedLocationObject != lastLocationObject) { - - accountManager.authenticatedRequest("/api/v1/users/address", QNetworkAccessManager::PutOperation, - JSONCallbackParameters(), QJsonDocument(updatedLocationObject).toJson()); - - lastLocationObject = updatedLocationObject; - } - } + accountManager.authenticatedRequest("/api/v1/users/address", QNetworkAccessManager::PutOperation, + JSONCallbackParameters(), QJsonDocument(updatedLocationObject).toJson()); + } } void Application::domainChanged(const QString& domainHostname) { diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index e51390f9d0..5f1b81fb14 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1897,7 +1897,12 @@ void MyAvatar::resetSize() { void MyAvatar::goToLocationFromResponse(const QJsonObject& jsonObject) { QJsonObject locationObject = jsonObject["data"].toObject()["address"].toObject(); - goToLocationFromAddress(locationObject); + bool isOnline = jsonObject["data"].toObject()["online"].toBool(); + if (isOnline ) { + goToLocationFromAddress(locationObject); + } else { + QMessageBox::warning(Application::getInstance()->getWindow(), "", "The user is not online."); + } } void MyAvatar::goToLocationFromAddress(const QJsonObject& locationObject) { diff --git a/libraries/octree/src/Octree.cpp b/libraries/octree/src/Octree.cpp index 2af86663f7..c11d23c2ec 100644 --- a/libraries/octree/src/Octree.cpp +++ b/libraries/octree/src/Octree.cpp @@ -764,7 +764,7 @@ bool findShapeCollisionsOp(OctreeElement* element, void* extraData) { return true; // recurse on children } if (element->hasContent()) { - if (ShapeCollider::collideShapeWithAACube(args->shape, cube.calcCenter(), cube.getScale(), args->collisions)) { + if (ShapeCollider::collideShapeWithAACubeLegacy(args->shape, cube.calcCenter(), cube.getScale(), args->collisions)) { args->found = true; return true; } diff --git a/libraries/shared/src/AACubeShape.cpp b/libraries/shared/src/AACubeShape.cpp new file mode 100644 index 0000000000..70477d5682 --- /dev/null +++ b/libraries/shared/src/AACubeShape.cpp @@ -0,0 +1,16 @@ +// +// AACubeShape.cpp +// libraries/shared/src +// +// Created by Andrew Meadows on 2014.08.22 +// 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 "AACubeShape.h" + +bool AACubeShape::findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const { + return false; +} diff --git a/libraries/shared/src/AACubeShape.h b/libraries/shared/src/AACubeShape.h new file mode 100644 index 0000000000..96010926c7 --- /dev/null +++ b/libraries/shared/src/AACubeShape.h @@ -0,0 +1,36 @@ +// +// AACubeShape.h +// libraries/shared/src +// +// Created by Andrew Meadows on 2014.08.22 +// 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_AACubeShape_h +#define hifi_AACubeShape_h + +#include "Shape.h" + +class AACubeShape : public Shape { +public: + AACubeShape() : Shape(AACUBE_SHAPE), _scale(1.0f) { } + AACubeShape(float scale) : Shape(AACUBE_SHAPE), _scale(scale) { } + AACubeShape(float scale, const glm::vec3& position) : Shape(AACUBE_SHAPE, position), _scale(scale) { } + + virtual ~AACubeShape() { } + + float getScale() const { return _scale; } + void setScale(float scale) { _scale = scale; } + + bool findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const; + + float getVolume() const { return _scale * _scale * _scale; } + +protected: + float _scale; +}; + +#endif // hifi_AACubeShape_h diff --git a/libraries/shared/src/Shape.h b/libraries/shared/src/Shape.h index 9c30a0fdf4..52369139c0 100644 --- a/libraries/shared/src/Shape.h +++ b/libraries/shared/src/Shape.h @@ -25,8 +25,9 @@ const float MAX_SHAPE_MASS = 1.0e18f; // something less than sqrt(FLT_MAX) const quint8 SPHERE_SHAPE = 0; const quint8 CAPSULE_SHAPE = 1; const quint8 PLANE_SHAPE = 2; -const quint8 LIST_SHAPE = 3; -const quint8 UNKNOWN_SHAPE = 4; +const quint8 AACUBE_SHAPE = 3; +const quint8 LIST_SHAPE = 4; +const quint8 UNKNOWN_SHAPE = 5; class Shape { public: diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index ec0c88bd0f..399d6e3d42 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -9,13 +9,13 @@ // See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html // -#include - #include -#include "GeometryUtil.h" #include "ShapeCollider.h" + +#include "AACubeShape.h" #include "CapsuleShape.h" +#include "GeometryUtil.h" #include "ListShape.h" #include "PlaneShape.h" #include "SphereShape.h" @@ -25,8 +25,8 @@ // * Large ListShape's are inefficient keep the lists short. // * Collisions between lists of lists work in theory but are not recommended. -const Shape::Type NUM_SHAPE_TYPES = 5; -const quint8 NUM__DISPATCH_CELLS = NUM_SHAPE_TYPES * NUM_SHAPE_TYPES; +const quint8 NUM_SHAPE_TYPES = UNKNOWN_SHAPE; +const quint8 NUM_DISPATCH_CELLS = NUM_SHAPE_TYPES * NUM_SHAPE_TYPES; Shape::Type getDispatchKey(Shape::Type typeA, Shape::Type typeB) { return typeA + NUM_SHAPE_TYPES * typeB; @@ -38,36 +38,44 @@ bool notImplemented(const Shape* shapeA, const Shape* shapeB, CollisionList& col } // NOTE: hardcode the number of dispatchTable entries (NUM_SHAPE_TYPES ^2) -bool (*dispatchTable[NUM__DISPATCH_CELLS])(const Shape*, const Shape*, CollisionList&); +bool (*dispatchTable[NUM_DISPATCH_CELLS])(const Shape*, const Shape*, CollisionList&); namespace ShapeCollider { // NOTE: the dispatch table must be initialized before the ShapeCollider is used. void initDispatchTable() { - for (Shape::Type i = 0; i < NUM__DISPATCH_CELLS; ++i) { + for (Shape::Type i = 0; i < NUM_DISPATCH_CELLS; ++i) { dispatchTable[i] = ¬Implemented; } - // NOTE: no need to update any that are notImplemented, but we leave them - // commented out in the code so that we remember that they exist. dispatchTable[getDispatchKey(SPHERE_SHAPE, SPHERE_SHAPE)] = &sphereVsSphere; dispatchTable[getDispatchKey(SPHERE_SHAPE, CAPSULE_SHAPE)] = &sphereVsCapsule; dispatchTable[getDispatchKey(SPHERE_SHAPE, PLANE_SHAPE)] = &sphereVsPlane; + dispatchTable[getDispatchKey(SPHERE_SHAPE, AACUBE_SHAPE)] = &sphereVsAACube; dispatchTable[getDispatchKey(SPHERE_SHAPE, LIST_SHAPE)] = &shapeVsList; dispatchTable[getDispatchKey(CAPSULE_SHAPE, SPHERE_SHAPE)] = &capsuleVsSphere; dispatchTable[getDispatchKey(CAPSULE_SHAPE, CAPSULE_SHAPE)] = &capsuleVsCapsule; dispatchTable[getDispatchKey(CAPSULE_SHAPE, PLANE_SHAPE)] = &capsuleVsPlane; + dispatchTable[getDispatchKey(CAPSULE_SHAPE, AACUBE_SHAPE)] = &capsuleVsAACube; dispatchTable[getDispatchKey(CAPSULE_SHAPE, LIST_SHAPE)] = &shapeVsList; dispatchTable[getDispatchKey(PLANE_SHAPE, SPHERE_SHAPE)] = &planeVsSphere; dispatchTable[getDispatchKey(PLANE_SHAPE, CAPSULE_SHAPE)] = &planeVsCapsule; dispatchTable[getDispatchKey(PLANE_SHAPE, PLANE_SHAPE)] = &planeVsPlane; + dispatchTable[getDispatchKey(PLANE_SHAPE, AACUBE_SHAPE)] = ¬Implemented; dispatchTable[getDispatchKey(PLANE_SHAPE, LIST_SHAPE)] = &shapeVsList; + dispatchTable[getDispatchKey(AACUBE_SHAPE, SPHERE_SHAPE)] = &aaCubeVsSphere; + dispatchTable[getDispatchKey(AACUBE_SHAPE, CAPSULE_SHAPE)] = &aaCubeVsCapsule; + dispatchTable[getDispatchKey(AACUBE_SHAPE, PLANE_SHAPE)] = ¬Implemented; + dispatchTable[getDispatchKey(AACUBE_SHAPE, AACUBE_SHAPE)] = &aaCubeVsAACube; + dispatchTable[getDispatchKey(AACUBE_SHAPE, LIST_SHAPE)] = &shapeVsList; + dispatchTable[getDispatchKey(LIST_SHAPE, SPHERE_SHAPE)] = &listVsShape; dispatchTable[getDispatchKey(LIST_SHAPE, CAPSULE_SHAPE)] = &listVsShape; dispatchTable[getDispatchKey(LIST_SHAPE, PLANE_SHAPE)] = &listVsShape; + dispatchTable[getDispatchKey(LIST_SHAPE, AACUBE_SHAPE)] = &listVsShape; dispatchTable[getDispatchKey(LIST_SHAPE, LIST_SHAPE)] = &listVsList; // all of the UNKNOWN_SHAPE pairings are notImplemented @@ -117,12 +125,12 @@ bool collideShapesWithShapes(const QVector& shapesA, const QVectorgetType(); if (typeA == SPHERE_SHAPE) { - return sphereVsAACube(static_cast(shapeA), cubeCenter, cubeSide, collisions); + return sphereVsAACubeLegacy(static_cast(shapeA), cubeCenter, cubeSide, collisions); } else if (typeA == CAPSULE_SHAPE) { - return capsuleVsAACube(static_cast(shapeA), cubeCenter, cubeSide, collisions); + return capsuleVsAACubeLegacy(static_cast(shapeA), cubeCenter, cubeSide, collisions); } else if (typeA == LIST_SHAPE) { const ListShape* listA = static_cast(shapeA); bool touching = false; @@ -130,9 +138,9 @@ bool collideShapeWithAACube(const Shape* shapeA, const glm::vec3& cubeCenter, fl const Shape* subShape = listA->getSubShape(i); int subType = subShape->getType(); if (subType == SPHERE_SHAPE) { - touching = sphereVsAACube(static_cast(subShape), cubeCenter, cubeSide, collisions) || touching; + touching = sphereVsAACubeLegacy(static_cast(subShape), cubeCenter, cubeSide, collisions) || touching; } else if (subType == CAPSULE_SHAPE) { - touching = capsuleVsAACube(static_cast(subShape), cubeCenter, cubeSide, collisions) || touching; + touching = capsuleVsAACubeLegacy(static_cast(subShape), cubeCenter, cubeSide, collisions) || touching; } } return touching; @@ -162,8 +170,8 @@ bool sphereVsSphere(const Shape* shapeA, const Shape* shapeB, CollisionList& col collision->_penetration = BA * (totalRadius - distance); // contactPoint is on surface of A collision->_contactPoint = sphereA->getTranslation() + sphereA->getRadius() * BA; - collision->_shapeA = sphereA; - collision->_shapeB = sphereB; + collision->_shapeA = shapeA; + collision->_shapeB = shapeB; return true; } } @@ -210,8 +218,8 @@ bool sphereVsCapsule(const Shape* shapeA, const Shape* shapeB, CollisionList& co collision->_penetration = (totalRadius - radialDistance) * radialAxis; // points from A into B // contactPoint is on surface of sphereA collision->_contactPoint = sphereA->getTranslation() + sphereA->getRadius() * radialAxis; - collision->_shapeA = sphereA; - collision->_shapeB = capsuleB; + collision->_shapeA = shapeA; + collision->_shapeB = shapeB; } else { // A is on B's axis, so the penetration is undefined... if (absAxialDistance > capsuleB->getHalfHeight()) { @@ -233,8 +241,8 @@ bool sphereVsCapsule(const Shape* shapeA, const Shape* shapeB, CollisionList& co collision->_penetration = (sign * (totalRadius + capsuleB->getHalfHeight() - absAxialDistance)) * capsuleAxis; // contactPoint is on surface of sphereA collision->_contactPoint = sphereA->getTranslation() + (sign * sphereA->getRadius()) * capsuleAxis; - collision->_shapeA = sphereA; - collision->_shapeB = capsuleB; + collision->_shapeA = shapeA; + collision->_shapeB = shapeB; } return true; } @@ -252,8 +260,8 @@ bool sphereVsPlane(const Shape* shapeA, const Shape* shapeB, CollisionList& coll } collision->_penetration = penetration; collision->_contactPoint = sphereA->getTranslation() + sphereA->getRadius() * glm::normalize(penetration); - collision->_shapeA = sphereA; - collision->_shapeB = planeB; + collision->_shapeA = shapeA; + collision->_shapeB = shapeB; return true; } return false; @@ -415,8 +423,8 @@ bool capsuleVsCapsule(const Shape* shapeA, const Shape* shapeB, CollisionList& c collision->_penetration = BA * (totalRadius - distance); // contactPoint is on surface of A collision->_contactPoint = centerA + distanceA * axisA + capsuleA->getRadius() * BA; - collision->_shapeA = capsuleA; - collision->_shapeB = capsuleB; + collision->_shapeA = shapeA; + collision->_shapeB = shapeB; return true; } } else { @@ -482,8 +490,8 @@ bool capsuleVsCapsule(const Shape* shapeA, const Shape* shapeB, CollisionList& c // average the internal pair, and then do the math from centerB collision->_contactPoint = centerB + (0.5f * (points[1] + points[2])) * axisB + (capsuleA->getRadius() - distance) * BA; - collision->_shapeA = capsuleA; - collision->_shapeB = capsuleB; + collision->_shapeA = shapeA; + collision->_shapeB = shapeB; return true; } } @@ -505,8 +513,8 @@ bool capsuleVsPlane(const Shape* shapeA, const Shape* shapeB, CollisionList& col collision->_penetration = penetration; glm::vec3 deepestEnd = (glm::dot(start, glm::vec3(plane)) < glm::dot(end, glm::vec3(plane))) ? start : end; collision->_contactPoint = deepestEnd + capsuleA->getRadius() * glm::normalize(penetration); - collision->_shapeA = capsuleA; - collision->_shapeB = planeB; + collision->_shapeA = shapeA; + collision->_shapeB = shapeB; return true; } return false; @@ -526,41 +534,8 @@ bool planeVsPlane(const Shape* shapeA, const Shape* shapeB, CollisionList& colli return false; } -bool shapeVsList(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - bool touching = false; - const ListShape* listB = static_cast(shapeB); - for (int i = 0; i < listB->size() && !collisions.isFull(); ++i) { - const Shape* subShape = listB->getSubShape(i); - touching = collideShapes(shapeA, subShape, collisions) || touching; - } - return touching; -} - -bool listVsShape(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - bool touching = false; - const ListShape* listA = static_cast(shapeA); - for (int i = 0; i < listA->size() && !collisions.isFull(); ++i) { - const Shape* subShape = listA->getSubShape(i); - touching = collideShapes(subShape, shapeB, collisions) || touching; - } - return touching; -} - -bool listVsList(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - bool touching = false; - const ListShape* listA = static_cast(shapeA); - const ListShape* listB = static_cast(shapeB); - for (int i = 0; i < listA->size() && !collisions.isFull(); ++i) { - const Shape* subShape = listA->getSubShape(i); - for (int j = 0; j < listB->size() && !collisions.isFull(); ++j) { - touching = collideShapes(subShape, listB->getSubShape(j), collisions) || touching; - } - } - return touching; -} - // helper function -bool sphereVsAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& cubeCenter, +bool sphereVsAACubeLegacy(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) { // sphere is A // cube is B @@ -656,6 +631,391 @@ bool sphereVsAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm return false; } +// 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 + 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 NULL; + } + CollisionInfo* collision = collisions.getNewCollision(); + if (!collision) { + return NULL; + } + 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 NULL; + } + 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->_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) + 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->_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 capsuleVsAACubeFace(const CapsuleShape* capsuleA, const AACubeShape* cubeB, int faceIndex, const glm::vec3& faceNormal, CollisionList& collisions) { + // we only fall in here when the capsuleAxis is nearly parallel to the face of a cube + glm::vec3 capsuleAxis; + capsuleA->computeNormalizedAxis(capsuleAxis); + glm::vec3 cubeCenter = cubeB->getTranslation(); + + // Revisualize the capsule as a line segment between two points. We'd like to collide the + // capsule as two spheres located at the endpoints or where the line segment hits the boundary + // of the face. + + // We raytrace forward into the four planes that neigbhor the face to find the boundary + // points of the line segment. + glm::vec3 capsuleStart; + capsuleA->getStartPoint(capsuleStart); + + // translate into cube-relative frame + capsuleStart -= cubeCenter; + float capsuleLength = 2.0f * capsuleA->getHalfHeight(); + float halfCubeSide = 0.5f * cubeB->getScale(); + float capsuleRadius = capsuleA->getRadius(); + + // preload distances with values that work for when the capsuleAxis runs parallel to neighbor face + float distances[] = {FLT_MAX, -FLT_MAX, FLT_MAX, -FLT_MAX, 0.0f}; + + // Loop over the directions that are NOT parallel to face (there are two of them). + // For each direction we'll raytrace against the positive and negative planes to find where + // the axis passes through. + for (int i = 0; i < 2; ++i) { + int wallIndex = wallIndices[2 * faceIndex + i]; + glm::vec3 wallNormal = cubeNormals[wallIndex]; + // each direction has two walls: positive and negative + float axisDotWall = glm::dot(capsuleAxis, wallNormal); + if (fabsf(axisDotWall) > EPSILON) { + // formula for distance to intersection between line (P,p) and plane (V,n) is: (V-P)*n / p*n + distances[2 * i] = (halfCubeSide - glm::dot(capsuleStart, wallNormal)) / axisDotWall; + distances[2 * i + 1] = -(halfCubeSide + glm::dot(capsuleStart, wallNormal)) / axisDotWall; + } + } + + // sort the distances from large to small + int j = 3; + while (j > 0) { + for (int i = 0; i <= j; ++i) { + if (distances[i] < distances[i+1]) { + // swap (using distances[4] as temp space) + distances[4] = distances[i]; + distances[i] = distances[i+1]; + distances[i+1] = distances[4]; + } + } + --j; + } + + // the capsule overlaps when the max of the mins is less than the min of the maxes + distances[0] = glm::min(capsuleLength, distances[1]); // maxDistance + distances[1] = glm::max(0.0f, distances[2]); // minDistance + bool hit = false; + if (distances[1] < distances[0]) { + // if we collide at all it will be at two points + for (int i = 0; i < 2; ++i) { + glm::vec3 sphereCenter = cubeCenter + capsuleStart + distances[i] * capsuleAxis; + // collide like a sphere at point0 with capsuleRadius + CollisionInfo* collision = sphereVsAACubeHelper(sphereCenter, capsuleRadius, + cubeCenter, 2.0f * halfCubeSide, collisions); + if (collision) { + // we hit! so store back pointers to the shapes + collision->_shapeA = capsuleA; + collision->_shapeB = cubeB; + hit = true; + } + } + return hit; + } else if (distances[1] < capsuleLength + capsuleRadius ) { + // we might collide at the end cap + glm::vec3 sphereCenter = cubeCenter + capsuleStart + capsuleLength * capsuleAxis; + // collide like a sphere at point0 with capsuleRadius + CollisionInfo* collision = sphereVsAACubeHelper(sphereCenter, capsuleRadius, + cubeCenter, 2.0f * halfCubeSide, collisions); + if (collision) { + // we hit! so store back pointers to the shapes + collision->_shapeA = capsuleA; + collision->_shapeB = cubeB; + hit = true; + } + } else if (distances[0] > -capsuleLength) { + // we might collide at the start cap + glm::vec3 sphereCenter = cubeCenter + capsuleStart; + // collide like a sphere at point0 with capsuleRadius + CollisionInfo* collision = sphereVsAACubeHelper(sphereCenter, capsuleRadius, + cubeCenter, 2.0f * halfCubeSide, collisions); + if (collision) { + // we hit! so store back pointers to the shapes + collision->_shapeA = capsuleA; + collision->_shapeB = cubeB; + hit = true; + } + } + return hit; +} + +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's line segment to cube's center + glm::vec3 capsuleAxis; + capsuleA->computeNormalizedAxis(capsuleAxis); + float halfHeight = capsuleA->getHalfHeight(); + glm::vec3 cubeCenter = cubeB->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 = capsuleCenter + 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); + } + + if (fabs(glm::dot(faceNormal, capsuleAxis)) < EPSILON) { + if (glm::dot(nearestApproach, faceNormal) > cubeB->getScale() + capsuleA->getRadius()) { + return false; + } + // we expect this case to be rare but complicated enough that we split it out + // into its own helper function + return capsuleVsAACubeFace(capsuleA, cubeB, faceIndex, faceNormal, collisions); + } + + // 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 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 + 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) { + // 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); + } + } + if (newDistance < shortestDistance) { + shortestDistance = newDistance; + } + // there can only be one hit per direction + break; + } + } + } + + // chose the point that produces the deepest penetration against face + // and translate back into real frame + glm::vec3 sphereCenter = cubeCenter + capsuleStart + shortestDistance * capsuleAxis; + + // collide like a sphere at point0 with capsuleRadius + CollisionInfo* collision = sphereVsAACubeHelper(sphereCenter, 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; + } + return false; +} + +bool aaCubeVsSphere(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { + return sphereVsAACube(shapeB, shapeA, collisions); +} + +bool aaCubeVsCapsule(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { + return capsuleVsAACube(shapeB, shapeA, collisions); +} + +bool aaCubeVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { + return false; +} + +bool shapeVsList(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { + bool touching = false; + const ListShape* listB = static_cast(shapeB); + for (int i = 0; i < listB->size() && !collisions.isFull(); ++i) { + const Shape* subShape = listB->getSubShape(i); + touching = collideShapes(shapeA, subShape, collisions) || touching; + } + return touching; +} + +bool listVsShape(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { + bool touching = false; + const ListShape* listA = static_cast(shapeA); + for (int i = 0; i < listA->size() && !collisions.isFull(); ++i) { + const Shape* subShape = listA->getSubShape(i); + touching = collideShapes(subShape, shapeB, collisions) || touching; + } + return touching; +} + +bool listVsList(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { + bool touching = false; + const ListShape* listA = static_cast(shapeA); + const ListShape* listB = static_cast(shapeB); + for (int i = 0; i < listA->size() && !collisions.isFull(); ++i) { + const Shape* subShape = listA->getSubShape(i); + for (int j = 0; j < listB->size() && !collisions.isFull(); ++j) { + touching = collideShapes(subShape, listB->getSubShape(j), collisions) || touching; + } + } + return touching; +} + // 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. @@ -708,11 +1068,11 @@ 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 sphereVsAACubeLegacy(const SphereShape* sphereA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) { + return sphereVsAACubeLegacy(sphereA->getTranslation(), sphereA->getRadius(), cubeCenter, cubeSide, collisions); } -bool capsuleVsAACube(const CapsuleShape* capsuleA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) { +bool capsuleVsAACubeLegacy(const CapsuleShape* capsuleA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) { // find nerest approach of capsule line segment to cube glm::vec3 capsuleAxis; capsuleA->computeNormalizedAxis(capsuleAxis); @@ -725,7 +1085,7 @@ bool capsuleVsAACube(const CapsuleShape* capsuleA, const glm::vec3& cubeCenter, } 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 sphereVsAACubeLegacy(nearestApproach, capsuleA->getRadius(), cubeCenter, cubeSide, collisions); } bool findRayIntersectionWithShapes(const QVector shapes, const glm::vec3& rayStart, const glm::vec3& rayDirection, float& minDistance) { diff --git a/libraries/shared/src/ShapeCollider.h b/libraries/shared/src/ShapeCollider.h index 279cbe3810..3cfec4c8a2 100644 --- a/libraries/shared/src/ShapeCollider.h +++ b/libraries/shared/src/ShapeCollider.h @@ -40,7 +40,7 @@ namespace ShapeCollider { /// \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); + bool collideShapeWithAACubeLegacy(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) @@ -95,6 +95,22 @@ namespace ShapeCollider { /// \param[out] collisions where to append collision details /// \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); + bool aaCubeVsCapsule(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions); + bool aaCubeVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions); /// \param shapeA pointer to first shape (cannot be NULL) /// \param listB pointer to second shape (cannot be NULL) @@ -119,14 +135,14 @@ namespace ShapeCollider { /// \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); + bool sphereVsAACubeLegacy(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); + bool capsuleVsAACubeLegacy(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 diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index dd2e6e396e..862792d8ac 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -1,5 +1,6 @@ # add the test directories file(GLOB TEST_SUBDIRS RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}/*") +list(REMOVE_ITEM TEST_SUBDIRS "CMakeFiles") foreach(DIR ${TEST_SUBDIRS}) if(IS_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/${DIR}") add_subdirectory(${DIR}) diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp index 45d3ed6508..88cb9fa548 100644 --- a/tests/physics/src/ShapeColliderTests.cpp +++ b/tests/physics/src/ShapeColliderTests.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include #include @@ -680,84 +681,210 @@ void ShapeColliderTests::capsuleTouchesCapsule() { } } +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; + + 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) { + 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) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube edge." + << " 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); + + // 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) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube corner." + << " cornerNormal = " << cornerNormal << std::endl; + break; + } + } + } + } + } +} + 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 outside cube face." + << " 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 inside cube face." + << " 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,123 +893,914 @@ 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; - } - - 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; + 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::sphereMissesAACube() { +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.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); + + 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; + } + } + } + } + } +} + +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); - 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; + // 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 + if (ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should NOT collide with cube face." + << " faceNormal = " << faceNormal << std::endl; + } + } } + + // 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 + 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; + } + } + } + } + } + + // 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; - // 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; + // 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 + if (ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should NOT collide with cube face." + << " cornerNormal = " << cornerNormal << 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; + // 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 + if (ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should NOT collide with cube" + << " edgeNormal = " << edgeNormal << std::endl; + } + } + } + } } - // 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; + // 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 + if (ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should NOT collide with cube" + << " cornerNormal = " << cornerNormal << std::endl; + } + } + } } - // 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; - } + // 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 }; - // 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; + 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 + if (ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should NOT collide with cube" + << " faceNormal = " << faceNormal << std::endl; + break; + } + } + } } } +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 + if (!ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions)) { + 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 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 + if (!ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions)) { + 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 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 + if (!ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions)) { + 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 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 + if (!ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions)) { + 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 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 + if (!ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions)) { + 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; + } + } + } + } + + // 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 + if (!ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should collide with cube" + << " faceNormal = " << faceNormal << std::endl; + break; + } + + int numCollisions = collisions.size(); + if (numCollisions != 2) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule should hit cube face at two spots." + << " Expected collisions size of 2 but is actually " << numCollisions + << ". faceNormal = " << faceNormal << std::endl; + break; + } + + // 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; + 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; + } + + // 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 + inaccuracy = (length0 < length1) ? length0 : length1; + if (fabs(inaccuracy) > allowableError) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contact: expectedContactPoint[" << k << "] = " << expectedContactPoint + << " actual = " << collision->_contactPoint + << " faceNormal = " << faceNormal + << std::endl; + } + } + } + } + } +} + + void ShapeColliderTests::rayHitsSphere() { float startDistance = 3.0f; glm::vec3 rayStart(-startDistance, 0.0f, 0.0f); @@ -1345,9 +2263,13 @@ void ShapeColliderTests::runAllTests() { capsuleMissesCapsule(); capsuleTouchesCapsule(); + sphereMissesAACube(); sphereTouchesAACubeFaces(); sphereTouchesAACubeEdges(); - sphereMissesAACube(); + sphereTouchesAACubeCorners(); + + capsuleMissesAACube(); + capsuleTouchesAACube(); rayHitsSphere(); rayBarelyHitsSphere(); diff --git a/tests/physics/src/ShapeColliderTests.h b/tests/physics/src/ShapeColliderTests.h index 4a51651cb8..a7495d32bf 100644 --- a/tests/physics/src/ShapeColliderTests.h +++ b/tests/physics/src/ShapeColliderTests.h @@ -25,8 +25,12 @@ namespace ShapeColliderTests { void sphereTouchesAACubeFaces(); void sphereTouchesAACubeEdges(); + void sphereTouchesAACubeCorners(); void sphereMissesAACube(); + void capsuleMissesAACube(); + void capsuleTouchesAACube(); + void rayHitsSphere(); void rayBarelyHitsSphere(); void rayBarelyMissesSphere();