From f684608d1f0149fe90c4759e9f0b92a98314002f Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 25 Aug 2014 08:24:44 -0700 Subject: [PATCH 1/9] Added AACubeShape with stubbed collision functions --- libraries/shared/src/AACubeShape.cpp | 16 +++ libraries/shared/src/AACubeShape.h | 36 +++++ libraries/shared/src/Shape.h | 5 +- libraries/shared/src/ShapeCollider.cpp | 185 ++++++++++++++++++++++--- libraries/shared/src/ShapeCollider.h | 6 + 5 files changed, 224 insertions(+), 24 deletions(-) create mode 100644 libraries/shared/src/AACubeShape.cpp create mode 100644 libraries/shared/src/AACubeShape.h 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..7bf35d7933 --- /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..90e7da4822 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -13,9 +13,11 @@ #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 +27,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 +40,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 @@ -162,8 +172,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 +220,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 +243,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 +262,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 +425,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 +492,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 +515,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,6 +536,137 @@ bool planeVsPlane(const Shape* shapeA, const Shape* shapeB, CollisionList& colli return false; } +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); + + float halfCubeSide = 0.5f * cubeB->getScale(); + float sphereRadius = sphereA->getRadius(); + + glm::vec3 sphereCenter = shapeA->getTranslation(); + glm::vec3 cubeCenter = shapeB->getTranslation(); + glm::vec3 BA = cubeCenter - sphereCenter; + + float distance = glm::length(BA); + if (distance > EPSILON) { + float maxBA = glm::max(glm::max(glm::abs(BA.x), glm::abs(BA.y)), glm::abs(BA.z)); + if (maxBA > halfCubeSide + sphereRadius) { + // sphere misses cube entirely + return false; + } + CollisionInfo* collision = collisions.getNewCollision(); + if (!collision) { + return false; + } + if (maxBA > halfCubeSide) { + // sphere hits cube but its center is outside cube + + // compute contact anti-pole on cube (in cube frame) + glm::vec3 cubeContact = glm::abs(BA); + if (cubeContact.x > halfCubeSide) { + cubeContact.x = halfCubeSide; + } + if (cubeContact.y > halfCubeSide) { + cubeContact.y = halfCubeSide; + } + if (cubeContact.z > halfCubeSide) { + cubeContact.z = halfCubeSide; + } + glm::vec3 signs = glm::sign(BA); + cubeContact.x *= signs.x; + cubeContact.y *= signs.y; + cubeContact.z *= signs.z; + + // compute penetration direction + glm::vec3 direction = BA - cubeContact; + float lengthDirection = glm::length(direction); + if (lengthDirection < EPSILON) { + // sphereCenter is touching cube surface, so we can't use the difference between those two + // points to compute the penetration direction. Instead we use the unitary components of + // cubeContact. + direction = cubeContact / halfCubeSide; + glm::modf(BA, direction); + lengthDirection = glm::length(direction); + } else if (lengthDirection > sphereRadius) { + collisions.deleteLastCollision(); + return false; + } + direction /= lengthDirection; + + // compute collision details + collision->_contactPoint = sphereCenter + sphereRadius * direction; + collision->_penetration = sphereRadius * direction - (BA - cubeContact); + } else { + // sphere center is inside cube + // --> push out nearest face + glm::vec3 direction; + BA /= maxBA; + glm::modf(BA, direction); + float lengthDirection = glm::length(direction); + direction /= lengthDirection; + + // compute collision details + collision->_penetration = (halfCubeSide * lengthDirection + sphereRadius - maxBA * glm::dot(BA, direction)) * direction; + collision->_contactPoint = sphereCenter + sphereRadius * direction; + } + collision->_shapeA = shapeA; + collision->_shapeB = shapeB; + return true; + } else if (sphereRadius + halfCubeSide > distance) { + // NOTE: for cocentric approximation we collide sphere and cube as two spheres which means + // this algorithm will probably be wrong when both sphere and cube are very small (both ~EPSILON) + CollisionInfo* collision = collisions.getNewCollision(); + if (collision) { + // the penetration and contactPoint are undefined, so we pick a penetration direction (-yAxis) + collision->_penetration = (sphereRadius + halfCubeSide) * glm::vec3(0.0f, -1.0f, 0.0f); + // contactPoint is on surface of A + collision->_contactPoint = sphereCenter + collision->_penetration; + + collision->_shapeA = shapeA; + collision->_shapeB = shapeB; + return true; + } + } + return false; +} + +bool capsuleVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { + /* + // find nearest approach of capsule line segment to cube + const CapsuleShape* capsuleA = static_cast(shapeA); + const AACubeShape* cubeB = static_cast(shapeB); + + glm::vec3 capsuleAxis; + capsuleA->computeNormalizedAxis(capsuleAxis); + glm::vec3 cubeCenter = shapeB->getTranslation(); + float offset = glm::dot(cubeCenter - capsuleA->getTranslation(), capsuleAxis); + float halfHeight = capsuleA->getHalfHeight(); + if (offset > halfHeight) { + offset = halfHeight; + } else if (offset < -halfHeight) { + offset = -halfHeight; + } + glm::vec3 BA = cubeCenter - sphereCenter; + glm::vec3 nearestApproach = capsuleA->getTranslation() + offset * capsuleAxis; + // collide nearest approach like a sphere at that point + return sphereVsAACube(nearestApproach, capsuleA->getRadius(), cubeCenter, cubeSide, collisions); + */ + return false; +} + +bool aaCubeVsSphere(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { + return false; +} + +bool aaCubeVsCapsule(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { + return false; +} + +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); @@ -713,7 +854,7 @@ bool sphereVsAACube(const SphereShape* sphereA, const glm::vec3& cubeCenter, flo } bool capsuleVsAACube(const CapsuleShape* capsuleA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) { - // find nerest approach of capsule line segment to cube + // find nearest approach of capsule line segment to cube glm::vec3 capsuleAxis; capsuleA->computeNormalizedAxis(capsuleAxis); float offset = glm::dot(cubeCenter - capsuleA->getTranslation(), capsuleAxis); diff --git a/libraries/shared/src/ShapeCollider.h b/libraries/shared/src/ShapeCollider.h index 3aa795e6fa..261b0640db 100644 --- a/libraries/shared/src/ShapeCollider.h +++ b/libraries/shared/src/ShapeCollider.h @@ -99,6 +99,12 @@ 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); + + 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) From 681c526fe13e2b41d5b1bfeabd122c56a29cb155 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 25 Aug 2014 08:52:45 -0700 Subject: [PATCH 2/9] exploit symmetry of collision pairings --- libraries/shared/src/ShapeCollider.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index 90e7da4822..f793163552 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -633,10 +633,17 @@ bool sphereVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& col bool capsuleVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { /* - // find nearest approach of capsule line segment to cube const CapsuleShape* capsuleA = static_cast(shapeA); const AACubeShape* cubeB = static_cast(shapeB); + // find nearest approach of capsule line segment to cube center + + // use nearest approach to find approximate point on cube surface + + // find point on cube surface closest to capsule line segment + + // collide like point inside sphere + glm::vec3 capsuleAxis; capsuleA->computeNormalizedAxis(capsuleAxis); glm::vec3 cubeCenter = shapeB->getTranslation(); @@ -656,11 +663,11 @@ bool capsuleVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& co } bool aaCubeVsSphere(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - return false; + return sphereVsAACube(shapeB, shapeA, collisions); } bool aaCubeVsCapsule(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - return false; + return capsuleVsAACube(shapeB, shapeA, collisions); } bool aaCubeVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { From c6253bb51a0eb5265b14355248b60b6ebea49349 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 27 Aug 2014 12:09:41 -0700 Subject: [PATCH 3/9] AACube vs Sphere and Capsule collision tests Sphere is unit tested. Also removed the shape collision query against Octree --- interface/src/avatar/MyAvatar.cpp | 2 + libraries/octree/src/Octree.cpp | 4 + libraries/octree/src/Octree.h | 5 +- libraries/shared/src/ShapeCollider.cpp | 324 +++++++--------- libraries/shared/src/ShapeCollider.h | 31 +- tests/physics/src/ShapeColliderTests.cpp | 450 +++++++++++++++-------- tests/physics/src/ShapeColliderTests.h | 1 + 7 files changed, 456 insertions(+), 361 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 239ca16186..6109ad4ef6 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1564,6 +1564,7 @@ void MyAvatar::updateCollisionWithEnvironment(float deltaTime, float radius) { static CollisionList myCollisions(64); void MyAvatar::updateCollisionWithVoxels(float deltaTime, float radius) { + /* TODO: Andrew to reimplement this const float MAX_VOXEL_COLLISION_SPEED = 100.0f; float speed = glm::length(_velocity); if (speed > MAX_VOXEL_COLLISION_SPEED) { @@ -1670,6 +1671,7 @@ void MyAvatar::updateCollisionWithVoxels(float deltaTime, float radius) { //updateCollisionSound(myCollisions[0]->_penetration, deltaTime, VOXEL_COLLISION_FREQUENCY); } _trapDuration = isTrapped ? _trapDuration + deltaTime : 0.0f; + */ } void MyAvatar::applyHardCollision(const glm::vec3& penetration, float elasticity, float damping) { diff --git a/libraries/octree/src/Octree.cpp b/libraries/octree/src/Octree.cpp index 2af86663f7..ba76119fb7 100644 --- a/libraries/octree/src/Octree.cpp +++ b/libraries/octree/src/Octree.cpp @@ -751,6 +751,7 @@ bool findCapsulePenetrationOp(OctreeElement* element, void* extraData) { return false; } +/* TODO: Andrew to reimplement or purge this bool findShapeCollisionsOp(OctreeElement* element, void* extraData) { ShapeArgs* args = static_cast(extraData); @@ -771,6 +772,7 @@ bool findShapeCollisionsOp(OctreeElement* element, void* extraData) { } return false; } +*/ bool Octree::findCapsulePenetration(const glm::vec3& start, const glm::vec3& end, float radius, glm::vec3& penetration, Octree::lockType lockType, bool* accurateResult) { @@ -809,6 +811,7 @@ bool Octree::findCapsulePenetration(const glm::vec3& start, const glm::vec3& end return args.found; } +/* TODO: Andrew to reimplement or purge this bool Octree::findShapeCollisions(const Shape* shape, CollisionList& collisions, Octree::lockType lockType, bool* accurateResult) { @@ -839,6 +842,7 @@ bool Octree::findShapeCollisions(const Shape* shape, CollisionList& collisions, } return args.found; } +*/ class GetElementEnclosingArgs { public: diff --git a/libraries/octree/src/Octree.h b/libraries/octree/src/Octree.h index 7ab22598ef..978cffd724 100644 --- a/libraries/octree/src/Octree.h +++ b/libraries/octree/src/Octree.h @@ -277,8 +277,9 @@ public: bool findCapsulePenetration(const glm::vec3& start, const glm::vec3& end, float radius, glm::vec3& penetration, Octree::lockType lockType = Octree::TryLock, bool* accurateResult = NULL); - bool findShapeCollisions(const Shape* shape, CollisionList& collisions, - Octree::lockType = Octree::TryLock, bool* accurateResult = NULL); +// TODO: Andrew to reimplement or purge this +// bool findShapeCollisions(const Shape* shape, CollisionList& collisions, +// Octree::lockType = Octree::TryLock, bool* accurateResult = NULL); OctreeElement* getElementEnclosingPoint(const glm::vec3& point, Octree::lockType lockType = Octree::TryLock, bool* accurateResult = NULL); diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index f793163552..0a5d167ff7 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -9,8 +9,6 @@ // See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html // -#include - #include #include "ShapeCollider.h" @@ -127,29 +125,6 @@ bool collideShapesWithShapes(const QVector& shapesA, const QVectorgetType(); - if (typeA == SPHERE_SHAPE) { - return sphereVsAACube(static_cast(shapeA), cubeCenter, cubeSide, collisions); - } else if (typeA == CAPSULE_SHAPE) { - return capsuleVsAACube(static_cast(shapeA), cubeCenter, cubeSide, collisions); - } else if (typeA == LIST_SHAPE) { - const ListShape* listA = static_cast(shapeA); - bool touching = false; - for (int i = 0; i < listA->size() && !collisions.isFull(); ++i) { - const Shape* subShape = listA->getSubShape(i); - int subType = subShape->getType(); - if (subType == SPHERE_SHAPE) { - touching = sphereVsAACube(static_cast(subShape), cubeCenter, cubeSide, collisions) || touching; - } else if (subType == CAPSULE_SHAPE) { - touching = capsuleVsAACube(static_cast(subShape), cubeCenter, cubeSide, collisions) || touching; - } - } - return touching; - } - return false; -} - bool sphereVsSphere(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { const SphereShape* sphereA = static_cast(shapeA); const SphereShape* sphereB = static_cast(shapeB); @@ -536,28 +511,24 @@ bool planeVsPlane(const Shape* shapeA, const Shape* shapeB, CollisionList& colli return false; } -bool sphereVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { +// helper function +CollisionInfo* sphereVsAACubeHelper(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& cubeCenter, + float cubeSide, CollisionList& collisions) { + // sphere is A + // cube is B // BA = B - A = from center of A to center of B - const SphereShape* sphereA = static_cast(shapeA); - const AACubeShape* cubeB = static_cast(shapeB); - - float halfCubeSide = 0.5f * cubeB->getScale(); - float sphereRadius = sphereA->getRadius(); - - glm::vec3 sphereCenter = shapeA->getTranslation(); - glm::vec3 cubeCenter = shapeB->getTranslation(); + float halfCubeSide = 0.5f * cubeSide; glm::vec3 BA = cubeCenter - sphereCenter; - float distance = glm::length(BA); if (distance > EPSILON) { float maxBA = glm::max(glm::max(glm::abs(BA.x), glm::abs(BA.y)), glm::abs(BA.z)); if (maxBA > halfCubeSide + sphereRadius) { // sphere misses cube entirely - return false; + return NULL; } CollisionInfo* collision = collisions.getNewCollision(); if (!collision) { - return false; + return NULL; } if (maxBA > halfCubeSide) { // sphere hits cube but its center is outside cube @@ -590,7 +561,7 @@ bool sphereVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& col lengthDirection = glm::length(direction); } else if (lengthDirection > sphereRadius) { collisions.deleteLastCollision(); - return false; + return NULL; } direction /= lengthDirection; @@ -607,12 +578,14 @@ bool sphereVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& col direction /= lengthDirection; // compute collision details + collision->_floatData = cubeSide; + collision->_vecData = cubeCenter; collision->_penetration = (halfCubeSide * lengthDirection + sphereRadius - maxBA * glm::dot(BA, direction)) * direction; collision->_contactPoint = sphereCenter + sphereRadius * direction; } - collision->_shapeA = shapeA; - collision->_shapeB = shapeB; - return true; + collision->_shapeA = NULL; + collision->_shapeB = NULL; + return collision; } else if (sphereRadius + halfCubeSide > distance) { // NOTE: for cocentric approximation we collide sphere and cube as two spheres which means // this algorithm will probably be wrong when both sphere and cube are very small (both ~EPSILON) @@ -622,43 +595,145 @@ bool sphereVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& col collision->_penetration = (sphereRadius + halfCubeSide) * glm::vec3(0.0f, -1.0f, 0.0f); // contactPoint is on surface of A collision->_contactPoint = sphereCenter + collision->_penetration; - - collision->_shapeA = shapeA; - collision->_shapeB = shapeB; - return true; + collision->_shapeA = NULL; + collision->_shapeB = NULL; + return collision; } } + return NULL; +} + +bool sphereVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { + // BA = B - A = from center of A to center of B + const SphereShape* sphereA = static_cast(shapeA); + const AACubeShape* cubeB = static_cast(shapeB); + + CollisionInfo* collision = sphereVsAACubeHelper( sphereA->getTranslation(), sphereA->getRadius(), + cubeB->getTranslation(), cubeB->getScale(), collisions); + if (collision) { + collision->_shapeA = shapeA; + collision->_shapeB = shapeB; + return true; + } return false; } +glm::vec3 cubeNormals[3] = { glm::vec3(1.0f, 0.0f, 0.0f), glm::vec3(0.0f, 1.0f, 0.0f), glm::vec3(0.0f, 0.0f, 1.0f) }; + +// the wallIndices is a sequence of pairs that represent the OTHER directions for each cube face, +// hence the first pair is (1, 2) because the OTHER faces for xFace are (yFace, zFace) = (1, 2) +int wallIndices[] = { 1, 2, 0, 2, 0, 1 }; + bool capsuleVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - /* const CapsuleShape* capsuleA = static_cast(shapeA); const AACubeShape* cubeB = static_cast(shapeB); - // find nearest approach of capsule line segment to cube center - - // use nearest approach to find approximate point on cube surface - - // find point on cube surface closest to capsule line segment - - // collide like point inside sphere - - glm::vec3 capsuleAxis; + // find nearest approach of capsule's line segment to cube's center + glm::vec3 capsuleAxis; capsuleA->computeNormalizedAxis(capsuleAxis); - glm::vec3 cubeCenter = shapeB->getTranslation(); - float offset = glm::dot(cubeCenter - capsuleA->getTranslation(), capsuleAxis); float halfHeight = capsuleA->getHalfHeight(); - if (offset > halfHeight) { - offset = halfHeight; - } else if (offset < -halfHeight) { - offset = -halfHeight; + glm::vec3 cubeCenter = cubeB->getTranslation(); + glm::vec3 BA = cubeCenter - capsuleA->getTranslation(); + float axialOffset = glm::dot(capsuleAxis, BA); + if (fabsf(axialOffset) > halfHeight) { + axialOffset = (axialOffset < 0.0f) ? -halfHeight : halfHeight; + } + glm::vec3 nearestApproach = axialOffset * capsuleAxis; + + // transform nearestApproach into cube-relative frame + nearestApproach -= cubeCenter; + + // determine the face of nearest approach + glm::vec3 signs = glm::sign(nearestApproach); + int faceIndex = 0; + glm::vec3 faceNormal(signs.x, 0.0f, 0.0f); + float maxApproach = fabsf(nearestApproach.x); + if (maxApproach < fabsf(nearestApproach.y)) { + maxApproach = fabsf(nearestApproach.y); + faceIndex = 1; + faceNormal = glm::vec3(0.0f, signs.y, 0.0f); + } + if (maxApproach < fabsf(nearestApproach.z)) { + maxApproach = fabsf(nearestApproach.z); + faceIndex = 2; + faceNormal = glm::vec3(0.0f, 0.0f, signs.z); + } + + // Revisualize the capsule as a startPoint and an axis that points toward the cube face. + // We raytrace forward into the four planes that neigbhor the face to find the furthest + // point along the capsule's axis that might hit face. + glm::vec3 capsuleStart; + if (glm::dot(capsuleAxis, faceNormal) < 0.0f) { + capsuleA->getStartPoint(capsuleStart); + } else { + // NOTE: we want dot(capsuleAxis, faceNormal) to be negative which simplifies some + // logic below, so we pretend the end is the start thereby reversing its axis. + capsuleA->getEndPoint(capsuleStart); + capsuleAxis *= -1.0f; + } + // translate into cube-relative frame + capsuleStart -= cubeCenter; + float capsuleLength = 2.0f * halfHeight; + float halfCubeSide = 0.5f * cubeB->getScale(); + float capsuleRadius = capsuleA->getRadius(); + + // Loop over the directions that are NOT parallel to face (there are two of them). + // For each direction we'll raytrace along capsuleAxis to find point impact + // on the furthest face and clamp it to remain on the capsule's line segment + float distances[2] = { 0, capsuleLength}; + int numCompleteMisses = 0; + for (int i = 0; i < 2; ++i) { + int wallIndex = wallIndices[2 * faceIndex + i]; + // each direction has two walls: positive and negative + for (float wallSign = -1.0f; wallSign < 2.0f; wallSign += 2.0f) { + glm::vec3 wallNormal = wallSign * cubeNormals[wallIndex]; + float axisDotWall = glm::dot(capsuleAxis, wallNormal); + if (axisDotWall > EPSILON) { + float distance = (halfCubeSide + glm::dot(capsuleStart, wallNormal)) / axisDotWall; + distances[i] = glm::clamp(distance, 0.0f, capsuleLength); + if (distance == distances[i]) { + // the wall truncated the capsule which means there is a possibility that the capusule + // actually collides against the edge of the face, so we check for that case now + glm::vec3 impact = capsuleStart + distance * capsuleAxis; + float depth = glm::dot(impact, faceNormal) - halfCubeSide; + if (depth > 0.0f) { + if (depth < capsuleRadius) { + // need to recast against the diagonal plane: wall rotated away from the face + glm::vec3 diagonalNormal = INV_SQUARE_ROOT_OF_2 * (wallNormal - faceNormal); + distances[i] = glm::min(glm::dot(capsuleStart, diagonalNormal), 0.0f); + } else { + // capsule misses this wall by more than capsuleRadius + ++numCompleteMisses; + } + } + } + // there can't be more than one hit for any direction so we break + break; + } + } + } + if (numCompleteMisses == 2) { + return false; + } + + // chose the point that produces the deepest penetration against face + glm::vec3 point0 = capsuleStart + distances[0] * capsuleAxis; + glm::vec3 point1 = capsuleStart + distances[1] * capsuleAxis; + if (glm::dot(point0, faceNormal) > glm::dot(point1, faceNormal)) { + point0 = point1; + } + // move back into real frame + point0 += cubeCenter; + + // collide like a sphere at point0 with capsuleRadius + CollisionInfo* collision = sphereVsAACubeHelper(point0, capsuleRadius, + cubeCenter, 2.0f * halfCubeSide, collisions); + if (collision) { + // we hit! so store back pointers to the shapes + collision->_shapeA = shapeA; + collision->_shapeB = shapeB; + return true; } - glm::vec3 BA = cubeCenter - sphereCenter; - glm::vec3 nearestApproach = capsuleA->getTranslation() + offset * capsuleAxis; - // collide nearest approach like a sphere at that point - return sphereVsAACube(nearestApproach, capsuleA->getRadius(), cubeCenter, cubeSide, collisions); - */ return false; } @@ -707,103 +782,6 @@ bool listVsList(const Shape* shapeA, const Shape* shapeB, CollisionList& collisi return touching; } -// helper function -bool sphereVsAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& cubeCenter, - float cubeSide, CollisionList& collisions) { - // sphere is A - // cube is B - // BA = B - A = from center of A to center of B - float halfCubeSide = 0.5f * cubeSide; - glm::vec3 BA = cubeCenter - sphereCenter; - float distance = glm::length(BA); - if (distance > EPSILON) { - float maxBA = glm::max(glm::max(glm::abs(BA.x), glm::abs(BA.y)), glm::abs(BA.z)); - if (maxBA > halfCubeSide + sphereRadius) { - // sphere misses cube entirely - return false; - } - CollisionInfo* collision = collisions.getNewCollision(); - if (!collision) { - return false; - } - if (maxBA > halfCubeSide) { - // sphere hits cube but its center is outside cube - - // compute contact anti-pole on cube (in cube frame) - glm::vec3 cubeContact = glm::abs(BA); - if (cubeContact.x > halfCubeSide) { - cubeContact.x = halfCubeSide; - } - if (cubeContact.y > halfCubeSide) { - cubeContact.y = halfCubeSide; - } - if (cubeContact.z > halfCubeSide) { - cubeContact.z = halfCubeSide; - } - glm::vec3 signs = glm::sign(BA); - cubeContact.x *= signs.x; - cubeContact.y *= signs.y; - cubeContact.z *= signs.z; - - // compute penetration direction - glm::vec3 direction = BA - cubeContact; - float lengthDirection = glm::length(direction); - if (lengthDirection < EPSILON) { - // sphereCenter is touching cube surface, so we can't use the difference between those two - // points to compute the penetration direction. Instead we use the unitary components of - // cubeContact. - direction = cubeContact / halfCubeSide; - glm::modf(BA, direction); - lengthDirection = glm::length(direction); - } else if (lengthDirection > sphereRadius) { - collisions.deleteLastCollision(); - return false; - } - direction /= lengthDirection; - - // compute collision details - collision->_contactPoint = sphereCenter + sphereRadius * direction; - collision->_penetration = sphereRadius * direction - (BA - cubeContact); - } else { - // sphere center is inside cube - // --> push out nearest face - glm::vec3 direction; - BA /= maxBA; - glm::modf(BA, direction); - float lengthDirection = glm::length(direction); - direction /= lengthDirection; - - // compute collision details - collision->_floatData = cubeSide; - collision->_vecData = cubeCenter; - collision->_penetration = (halfCubeSide * lengthDirection + sphereRadius - maxBA * glm::dot(BA, direction)) * direction; - collision->_contactPoint = sphereCenter + sphereRadius * direction; - } - collision->_floatData = cubeSide; - collision->_vecData = cubeCenter; - collision->_shapeA = NULL; - collision->_shapeB = NULL; - return true; - } else if (sphereRadius + halfCubeSide > distance) { - // NOTE: for cocentric approximation we collide sphere and cube as two spheres which means - // this algorithm will probably be wrong when both sphere and cube are very small (both ~EPSILON) - CollisionInfo* collision = collisions.getNewCollision(); - if (collision) { - // the penetration and contactPoint are undefined, so we pick a penetration direction (-yAxis) - collision->_penetration = (sphereRadius + halfCubeSide) * glm::vec3(0.0f, -1.0f, 0.0f); - // contactPoint is on surface of A - collision->_contactPoint = sphereCenter + collision->_penetration; - - collision->_floatData = cubeSide; - collision->_vecData = cubeCenter; - collision->_shapeA = NULL; - collision->_shapeB = NULL; - return true; - } - } - return false; -} - // helper function /* KEEP THIS CODE -- this is how to collide the cube with stark face normals (no rounding). * We might want to use this code later for sealing boundaries between adjacent voxels. @@ -856,26 +834,6 @@ bool sphereAACube_StarkAngles(const glm::vec3& sphereCenter, float sphereRadius, } */ -bool sphereVsAACube(const SphereShape* sphereA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) { - return sphereVsAACube(sphereA->getTranslation(), sphereA->getRadius(), cubeCenter, cubeSide, collisions); -} - -bool capsuleVsAACube(const CapsuleShape* capsuleA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) { - // find nearest approach of capsule line segment to cube - glm::vec3 capsuleAxis; - capsuleA->computeNormalizedAxis(capsuleAxis); - float offset = glm::dot(cubeCenter - capsuleA->getTranslation(), capsuleAxis); - float halfHeight = capsuleA->getHalfHeight(); - if (offset > halfHeight) { - offset = halfHeight; - } else if (offset < -halfHeight) { - offset = -halfHeight; - } - glm::vec3 nearestApproach = capsuleA->getTranslation() + offset * capsuleAxis; - // collide nearest approach like a sphere at that point - return sphereVsAACube(nearestApproach, capsuleA->getRadius(), cubeCenter, cubeSide, collisions); -} - bool findRayIntersectionWithShapes(const QVector shapes, const glm::vec3& rayStart, const glm::vec3& rayDirection, float& minDistance) { float hitDistance = FLT_MAX; int numShapes = shapes.size(); diff --git a/libraries/shared/src/ShapeCollider.h b/libraries/shared/src/ShapeCollider.h index 261b0640db..41008b51e7 100644 --- a/libraries/shared/src/ShapeCollider.h +++ b/libraries/shared/src/ShapeCollider.h @@ -39,13 +39,6 @@ namespace ShapeCollider { bool collideShapeWithShapes(const Shape* shapeA, const QVector& shapes, int startIndex, CollisionList& collisions); bool collideShapesWithShapes(const QVector& shapesA, const QVector& shapesB, CollisionList& collisions); - /// \param shapeA a pointer to a shape (cannot be NULL) - /// \param cubeCenter center of cube - /// \param cubeSide lenght of side of cube - /// \param collisions[out] average collision details - /// \return true if shapeA collides with axis aligned cube - bool collideShapeWithAACube(const Shape* shapeA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions); - /// \param sphereA pointer to first shape (cannot be NULL) /// \param sphereB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details @@ -100,6 +93,16 @@ namespace ShapeCollider { /// \return true if shapes collide bool planeVsPlane(const Shape* planeA, const Shape* planeB, CollisionList& collisions); + /// helper function for *VsAACube() methods + /// \param sphereCenter center of sphere + /// \param sphereRadius radius of sphere + /// \param cubeCenter center of AACube + /// \param cubeSide scale of cube + /// \param[out] collisions where to append collision details + /// \return valid pointer to CollisionInfo if sphere and cube overlap or NULL if not + CollisionInfo* sphereVsAACubeHelper(const glm::vec3& sphereCenter, float sphereRadius, + const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions); + bool sphereVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions); bool capsuleVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions); bool aaCubeVsSphere(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions); @@ -124,20 +127,6 @@ namespace ShapeCollider { /// \return true if shapes collide bool listVsList(const Shape* listA, const Shape* listB, CollisionList& collisions); - /// \param sphereA pointer to sphere (cannot be NULL) - /// \param cubeCenter center of cube - /// \param cubeSide lenght of side of cube - /// \param[out] collisions where to append collision details - /// \return true if sphereA collides with axis aligned cube - bool sphereVsAACube(const SphereShape* sphereA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions); - - /// \param capsuleA pointer to capsule (cannot be NULL) - /// \param cubeCenter center of cube - /// \param cubeSide lenght of side of cube - /// \param[out] collisions where to append collision details - /// \return true if capsuleA collides with axis aligned cube - bool capsuleVsAACube(const CapsuleShape* capsuleA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions); - /// \param shapes list of pointers to shapes (shape pointers may be NULL) /// \param startPoint beginning of ray /// \param direction direction of ray diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp index 45d3ed6508..d6b1b9d751 100644 --- a/tests/physics/src/ShapeColliderTests.cpp +++ b/tests/physics/src/ShapeColliderTests.cpp @@ -683,81 +683,93 @@ void ShapeColliderTests::capsuleTouchesCapsule() { void ShapeColliderTests::sphereTouchesAACubeFaces() { CollisionList collisions(16); - glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f); - float cubeSide = 2.34f; - float sphereRadius = 1.13f; glm::vec3 sphereCenter(0.0f); - SphereShape sphere(sphereRadius, sphereCenter); - QVector axes; - axes.push_back(xAxis); - axes.push_back(-xAxis); - axes.push_back(yAxis); - axes.push_back(-yAxis); - axes.push_back(zAxis); - axes.push_back(-zAxis); + glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f); + float cubeSide = 4.34f; - for (int i = 0; i < axes.size(); ++i) { - glm::vec3 axis = axes[i]; - // outside - { - collisions.clear(); - float overlap = 0.25f; - float sphereOffset = 0.5f * cubeSide + sphereRadius - overlap; - sphereCenter = cubeCenter + sphereOffset * axis; - sphere.setTranslation(sphereCenter); - - if (!ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube. axis = " << axis - << std::endl; - } - CollisionInfo* collision = collisions[0]; - if (!collision) { - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: no CollisionInfo. axis = " << axis << std::endl; - } - - glm::vec3 expectedPenetration = - overlap * axis; - if (glm::distance(expectedPenetration, collision->_penetration) > EPSILON) { - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: penetration = " << collision->_penetration - << " expected " << expectedPenetration << " axis = " << axis << std::endl; - } - - glm::vec3 expectedContact = sphereCenter - sphereRadius * axis; - if (glm::distance(expectedContact, collision->_contactPoint) > EPSILON) { - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: contactaPoint = " << collision->_contactPoint - << " expected " << expectedContact << " axis = " << axis << std::endl; - } - } + glm::vec3 faceNormals[] = {xAxis, yAxis, zAxis}; + int numDirections = 3; - // inside - { - collisions.clear(); - float overlap = 1.25f * sphereRadius; - float sphereOffset = 0.5f * cubeSide + sphereRadius - overlap; - sphereCenter = cubeCenter + sphereOffset * axis; - sphere.setTranslation(sphereCenter); + for (int i = 0; i < numDirections; ++i) { + // loop over both sides of cube positive and negative + for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) { + glm::vec3 faceNormal = sign * faceNormals[i]; + // outside + { + collisions.clear(); + float overlap = 0.25f * sphereRadius; + float parallelOffset = 0.5f * cubeSide + sphereRadius - overlap; + float perpOffset = 0.25f * cubeSide; + glm::vec3 expectedPenetration = - overlap * faceNormal; - if (!ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube." - << " axis = " << axis << std::endl; - } - CollisionInfo* collision = collisions[0]; - if (!collision) { - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: no CollisionInfo on y-axis." - << " axis = " << axis << std::endl; + // We rotate the position of the sphereCenter about a circle on the cube face so that + // it hits the same face in multiple spots. The penetration should be invarient for + // all collisions. + float delta = TWO_PI / 4.0f; + for (float angle = 0; angle < TWO_PI + EPSILON; angle += delta) { + glm::quat rotation = glm::angleAxis(angle, faceNormal); + glm::vec3 perpAxis = rotation * faceNormals[(i + 1) % numDirections]; + + sphereCenter = cubeCenter + parallelOffset * faceNormal + perpOffset * perpAxis; + + CollisionInfo* collision = ShapeCollider::sphereVsAACubeHelper(sphereCenter, sphereRadius, + cubeCenter, cubeSide, collisions); + + if (!collision) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube. faceNormal = " << faceNormal + << std::endl; + break; + } + + if (glm::distance(expectedPenetration, collision->_penetration) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: penetration = " << collision->_penetration + << " expected " << expectedPenetration << " faceNormal = " << faceNormal << std::endl; + } + + glm::vec3 expectedContact = sphereCenter - sphereRadius * faceNormal; + if (glm::distance(expectedContact, collision->_contactPoint) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: contactaPoint = " << collision->_contactPoint + << " expected " << expectedContact << " faceNormal = " << faceNormal << std::endl; + } + + if (collision->getShapeA()) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: collision->_shapeA should be NULL" << std::endl; + } + if (collision->getShapeB()) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: collision->_shapeB should be NULL" << std::endl; + } + } } - glm::vec3 expectedPenetration = - overlap * axis; - if (glm::distance(expectedPenetration, collision->_penetration) > EPSILON) { - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: penetration = " << collision->_penetration - << " expected " << expectedPenetration << " axis = " << axis << std::endl; - } + // inside + { + collisions.clear(); + float overlap = 1.25f * sphereRadius; + float sphereOffset = 0.5f * cubeSide + sphereRadius - overlap; + sphereCenter = cubeCenter + sphereOffset * faceNormal; + + CollisionInfo* collision = ShapeCollider::sphereVsAACubeHelper(sphereCenter, sphereRadius, + cubeCenter, cubeSide, collisions); - glm::vec3 expectedContact = sphereCenter - sphereRadius * axis; - if (glm::distance(expectedContact, collision->_contactPoint) > EPSILON) { - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: contactaPoint = " << collision->_contactPoint - << " expected " << expectedContact << " axis = " << axis << std::endl; + if (!collision) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube." + << " faceNormal = " << faceNormal << std::endl; + break; + } + + glm::vec3 expectedPenetration = - overlap * faceNormal; + if (glm::distance(expectedPenetration, collision->_penetration) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: penetration = " << collision->_penetration + << " expected " << expectedPenetration << " faceNormal = " << faceNormal << std::endl; + } + + glm::vec3 expectedContact = sphereCenter - sphereRadius * faceNormal; + if (glm::distance(expectedContact, collision->_contactPoint) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: contactaPoint = " << collision->_contactPoint + << " expected " << expectedContact << " faceNormal = " << faceNormal << std::endl; + } } } } @@ -766,64 +778,133 @@ void ShapeColliderTests::sphereTouchesAACubeFaces() { void ShapeColliderTests::sphereTouchesAACubeEdges() { CollisionList collisions(20); - glm::vec3 cubeCenter(0.0f, 0.0f, 0.0f); - float cubeSide = 2.0f; - - float sphereRadius = 1.0f; + float sphereRadius = 1.37f; glm::vec3 sphereCenter(0.0f); - SphereShape sphere(sphereRadius, sphereCenter); - QVector axes; - // edges - axes.push_back(glm::vec3(0.0f, 1.0f, 1.0f)); - axes.push_back(glm::vec3(0.0f, 1.0f, -1.0f)); - axes.push_back(glm::vec3(0.0f, -1.0f, 1.0f)); - axes.push_back(glm::vec3(0.0f, -1.0f, -1.0f)); - axes.push_back(glm::vec3(1.0f, 1.0f, 0.0f)); - axes.push_back(glm::vec3(1.0f, -1.0f, 0.0f)); - axes.push_back(glm::vec3(-1.0f, 1.0f, 0.0f)); - axes.push_back(glm::vec3(-1.0f, -1.0f, 0.0f)); - axes.push_back(glm::vec3(1.0f, 0.0f, 1.0f)); - axes.push_back(glm::vec3(1.0f, 0.0f, -1.0f)); - axes.push_back(glm::vec3(-1.0f, 0.0f, 1.0f)); - axes.push_back(glm::vec3(-1.0f, 0.0f, -1.0f)); - // and corners - axes.push_back(glm::vec3(1.0f, 1.0f, 1.0f)); - axes.push_back(glm::vec3(1.0f, 1.0f, -1.0f)); - axes.push_back(glm::vec3(1.0f, -1.0f, 1.0f)); - axes.push_back(glm::vec3(1.0f, -1.0f, -1.0f)); - axes.push_back(glm::vec3(-1.0f, 1.0f, 1.0f)); - axes.push_back(glm::vec3(-1.0f, 1.0f, -1.0f)); - axes.push_back(glm::vec3(-1.0f, -1.0f, 1.0f)); - axes.push_back(glm::vec3(-1.0f, -1.0f, -1.0f)); + glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f); + float cubeSide = 2.98f; - for (int i =0; i < axes.size(); ++i) { - glm::vec3 axis = axes[i]; - float lengthAxis = glm::length(axis); - axis /= lengthAxis; - float overlap = 0.25f; - - sphereCenter = cubeCenter + (lengthAxis * 0.5f * cubeSide + sphereRadius - overlap) * axis; - sphere.setTranslation(sphereCenter); - - if (!ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube. axis = " << axis << std::endl; - } - CollisionInfo* collision = collisions[i]; - if (!collision) { - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: no CollisionInfo. axis = " << axis << std::endl; + float overlap = 0.25 * sphereRadius; + int numSteps = 5; + + glm::vec3 faceNormals[] = {xAxis, yAxis, zAxis}; + int numDirections = 3; + + // loop over each face... + for (int i = 0; i < numDirections; ++i) { + for (float faceSign = -1.0f; faceSign < 2.0f; faceSign += 2.0f) { + glm::vec3 faceNormal = faceSign * faceNormals[i]; + + // loop over each neighboring face... + for (int j = (i + 1) % numDirections; j != i; j = (j + 1) % numDirections) { + // Compute the index to the third direction, which points perpendicular to both the face + // and the neighbor face. + int k = (j + 1) % numDirections; + if (k == i) { + k = (i + 1) % numDirections; + } + glm::vec3 thirdNormal = faceNormals[k]; + + for (float neighborSign = -1.0f; neighborSign < 2.0f; neighborSign += 2.0f) { + collisions.clear(); + glm::vec3 neighborNormal = neighborSign * faceNormals[j]; + + // combine the face and neighbor normals to get the edge normal + glm::vec3 edgeNormal = glm::normalize(faceNormal + neighborNormal); + + // Step the sphere along the edge in the direction of thirdNormal, starting at one corner and + // moving to the other. Test the penetration (invarient) and contact (changing) at each point. + glm::vec3 expectedPenetration = - overlap * edgeNormal; + float delta = cubeSide / (float)(numSteps - 1); + glm::vec3 startPosition = cubeCenter + (0.5f * cubeSide) * (faceNormal + neighborNormal - thirdNormal); + for (int m = 0; m < numSteps; ++m) { + sphereCenter = startPosition + ((float)m * delta) * thirdNormal + (sphereRadius - overlap) * edgeNormal; + + CollisionInfo* collision = ShapeCollider::sphereVsAACubeHelper(sphereCenter, sphereRadius, + cubeCenter, cubeSide, collisions); + + if (!collision) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube edge." + << " edgeNormal = " << edgeNormal << std::endl; + break; + } + + if (glm::distance(expectedPenetration, collision->_penetration) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: penetration = " << collision->_penetration + << " expected " << expectedPenetration << " edgeNormal = " << edgeNormal << std::endl; + } + + glm::vec3 expectedContact = sphereCenter - sphereRadius * edgeNormal; + if (glm::distance(expectedContact, collision->_contactPoint) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: contactaPoint = " << collision->_contactPoint + << " expected " << expectedContact << " edgeNormal = " << edgeNormal << std::endl; + } + } + } + } } + } +} + +void ShapeColliderTests::sphereTouchesAACubeCorners() { + CollisionList collisions(20); - glm::vec3 expectedPenetration = - overlap * axis; - if (glm::distance(expectedPenetration, collision->_penetration) > EPSILON) { - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: penetration = " << collision->_penetration - << " expected " << expectedPenetration << " axis = " << axis << std::endl; - } + float sphereRadius = 1.37f; + glm::vec3 sphereCenter(0.0f); + + glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f); + float cubeSide = 2.98f; + + float overlap = 0.25 * sphereRadius; + int numSteps = 5; + + glm::vec3 faceNormals[] = {xAxis, yAxis, zAxis}; + + for (float firstSign = -1.0f; firstSign < 2.0f; firstSign += 2.0f) { + glm::vec3 firstNormal = firstSign * faceNormals[0]; + for (float secondSign = -1.0f; secondSign < 2.0f; secondSign += 2.0f) { + glm::vec3 secondNormal = secondSign * faceNormals[1]; + for (float thirdSign = -1.0f; thirdSign < 2.0f; thirdSign += 2.0f) { + collisions.clear(); + glm::vec3 thirdNormal = thirdSign * faceNormals[2]; + + // the cornerNormal is the normalized sum of the three faces + glm::vec3 cornerNormal = glm::normalize(firstNormal + secondNormal + thirdNormal); + + // compute a direction that is slightly offset from cornerNormal + glm::vec3 perpAxis = glm::normalize(glm::cross(cornerNormal, firstNormal)); + glm::vec3 nearbyAxis = glm::normalize(cornerNormal + 0.1f * perpAxis); + + // swing the sphere on a small cone that starts at the corner and is centered on the cornerNormal + float delta = TWO_PI / (float)(numSteps - 1); + for (int i = 0; i < numSteps; i++) { + float angle = (float)i * delta; + glm::quat rotation = glm::angleAxis(angle, cornerNormal); + glm::vec3 offsetAxis = rotation * nearbyAxis; + sphereCenter = cubeCenter + (SQUARE_ROOT_OF_3 * 0.5f * cubeSide) * cornerNormal + (sphereRadius - overlap) * offsetAxis; + + CollisionInfo* collision = ShapeCollider::sphereVsAACubeHelper(sphereCenter, sphereRadius, + cubeCenter, cubeSide, collisions); - glm::vec3 expectedContact = sphereCenter - sphereRadius * axis; - if (glm::distance(expectedContact, collision->_contactPoint) > EPSILON) { - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: contactaPoint = " << collision->_contactPoint - << " expected " << expectedContact << " axis = " << axis << std::endl; + if (!collision) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube corner." + << " cornerNormal = " << cornerNormal << std::endl; + break; + } + + glm::vec3 expectedPenetration = - overlap * offsetAxis; + if (glm::distance(expectedPenetration, collision->_penetration) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: penetration = " << collision->_penetration + << " expected " << expectedPenetration << " cornerNormal = " << cornerNormal << std::endl; + } + + glm::vec3 expectedContact = sphereCenter - sphereRadius * offsetAxis; + if (glm::distance(expectedContact, collision->_contactPoint) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: contactaPoint = " << collision->_contactPoint + << " expected " << expectedContact << " cornerNormal = " << cornerNormal << std::endl; + } + } + } } } } @@ -831,55 +912,113 @@ void ShapeColliderTests::sphereTouchesAACubeEdges() { void ShapeColliderTests::sphereMissesAACube() { CollisionList collisions(16); + float sphereRadius = 1.0f; + glm::vec3 sphereCenter(0.0f); + glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f); float cubeSide = 2.0f; - float sphereRadius = 1.0f; - glm::vec3 sphereCenter(0.0f); - SphereShape sphere(sphereRadius, sphereCenter); + glm::vec3 faceNormals[] = {xAxis, yAxis, zAxis}; + int numDirections = 3; - float sphereOffset = (0.5f * cubeSide + sphereRadius + 0.25f); + float offset = 2.0f * EPSILON; - // top - sphereCenter = cubeCenter + sphereOffset * yAxis; - sphere.setTranslation(sphereCenter); - if (ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube" << std::endl; + // faces + for (int i = 0; i < numDirections; ++i) { + for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) { + glm::vec3 faceNormal = sign * faceNormals[i]; + + sphereCenter = cubeCenter + (0.5f * cubeSide + sphereRadius + offset) * faceNormal; + + CollisionInfo* collision = ShapeCollider::sphereVsAACubeHelper(sphereCenter, sphereRadius, + cubeCenter, cubeSide, collisions); + + if (collision) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube face." + << " faceNormal = " << faceNormal << std::endl; + } + } } + + // edges + int numSteps = 5; + // loop over each face... + for (int i = 0; i < numDirections; ++i) { + for (float faceSign = -1.0f; faceSign < 2.0f; faceSign += 2.0f) { + glm::vec3 faceNormal = faceSign * faceNormals[i]; + + // loop over each neighboring face... + for (int j = (i + 1) % numDirections; j != i; j = (j + 1) % numDirections) { + // Compute the index to the third direction, which points perpendicular to both the face + // and the neighbor face. + int k = (j + 1) % numDirections; + if (k == i) { + k = (i + 1) % numDirections; + } + glm::vec3 thirdNormal = faceNormals[k]; + + for (float neighborSign = -1.0f; neighborSign < 2.0f; neighborSign += 2.0f) { + collisions.clear(); + glm::vec3 neighborNormal = neighborSign * faceNormals[j]; + + // combine the face and neighbor normals to get the edge normal + glm::vec3 edgeNormal = glm::normalize(faceNormal + neighborNormal); + // Step the sphere along the edge in the direction of thirdNormal, starting at one corner and + // moving to the other. Test the penetration (invarient) and contact (changing) at each point. + float delta = cubeSide / (float)(numSteps - 1); + glm::vec3 startPosition = cubeCenter + (0.5f * cubeSide) * (faceNormal + neighborNormal - thirdNormal); + for (int m = 0; m < numSteps; ++m) { + sphereCenter = startPosition + ((float)m * delta) * thirdNormal + (sphereRadius + offset) * edgeNormal; + + CollisionInfo* collision = ShapeCollider::sphereVsAACubeHelper(sphereCenter, sphereRadius, + cubeCenter, cubeSide, collisions); - // bottom - sphereCenter = cubeCenter - sphereOffset * yAxis; - sphere.setTranslation(sphereCenter); - if (ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube" << std::endl; + if (collision) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube edge." + << " edgeNormal = " << edgeNormal << std::endl; + } + } + + } + } + } } - // left - sphereCenter = cubeCenter + sphereOffset * xAxis; - sphere.setTranslation(sphereCenter); - if (ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube" << std::endl; - } + // corners + for (float firstSign = -1.0f; firstSign < 2.0f; firstSign += 2.0f) { + glm::vec3 firstNormal = firstSign * faceNormals[0]; + for (float secondSign = -1.0f; secondSign < 2.0f; secondSign += 2.0f) { + glm::vec3 secondNormal = secondSign * faceNormals[1]; + for (float thirdSign = -1.0f; thirdSign < 2.0f; thirdSign += 2.0f) { + collisions.clear(); + glm::vec3 thirdNormal = thirdSign * faceNormals[2]; - // right - sphereCenter = cubeCenter - sphereOffset * xAxis; - sphere.setTranslation(sphereCenter); - if (ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube" << std::endl; - } + // the cornerNormal is the normalized sum of the three faces + glm::vec3 cornerNormal = glm::normalize(firstNormal + secondNormal + thirdNormal); - // forward - sphereCenter = cubeCenter + sphereOffset * zAxis; - sphere.setTranslation(sphereCenter); - if (ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube" << std::endl; - } + // compute a direction that is slightly offset from cornerNormal + glm::vec3 perpAxis = glm::normalize(glm::cross(cornerNormal, firstNormal)); + glm::vec3 nearbyAxis = glm::normalize(cornerNormal + 0.3f * perpAxis); - // back - sphereCenter = cubeCenter - sphereOffset * zAxis; - sphere.setTranslation(sphereCenter); - if (ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube" << std::endl; + // swing the sphere on a small cone that starts at the corner and is centered on the cornerNormal + float delta = TWO_PI / (float)(numSteps - 1); + for (int i = 0; i < numSteps; i++) { + float angle = (float)i * delta; + glm::quat rotation = glm::angleAxis(angle, cornerNormal); + glm::vec3 offsetAxis = rotation * nearbyAxis; + sphereCenter = cubeCenter + (SQUARE_ROOT_OF_3 * 0.5f * cubeSide) * cornerNormal + (sphereRadius + offset) * offsetAxis; + + CollisionInfo* collision = ShapeCollider::sphereVsAACubeHelper(sphereCenter, sphereRadius, + cubeCenter, cubeSide, collisions); + + if (collision) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube corner." + << " cornerNormal = " << cornerNormal << std::endl; + break; + } + } + } + } } } @@ -1347,6 +1486,7 @@ void ShapeColliderTests::runAllTests() { sphereTouchesAACubeFaces(); sphereTouchesAACubeEdges(); + sphereTouchesAACubeCorners(); sphereMissesAACube(); rayHitsSphere(); diff --git a/tests/physics/src/ShapeColliderTests.h b/tests/physics/src/ShapeColliderTests.h index 4a51651cb8..16e0e0c409 100644 --- a/tests/physics/src/ShapeColliderTests.h +++ b/tests/physics/src/ShapeColliderTests.h @@ -25,6 +25,7 @@ namespace ShapeColliderTests { void sphereTouchesAACubeFaces(); void sphereTouchesAACubeEdges(); + void sphereTouchesAACubeCorners(); void sphereMissesAACube(); void rayHitsSphere(); From f1ae0ce99248fe550e4cb9800f91573a58759172 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 27 Aug 2014 12:21:03 -0700 Subject: [PATCH 4/9] moved some functions around --- tests/physics/src/ShapeColliderTests.cpp | 228 +++++++++++------------ 1 file changed, 114 insertions(+), 114 deletions(-) diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp index d6b1b9d751..0ae6ef2f47 100644 --- a/tests/physics/src/ShapeColliderTests.cpp +++ b/tests/physics/src/ShapeColliderTests.cpp @@ -680,6 +680,119 @@ 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); @@ -909,119 +1022,6 @@ void ShapeColliderTests::sphereTouchesAACubeCorners() { } } -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::rayHitsSphere() { float startDistance = 3.0f; glm::vec3 rayStart(-startDistance, 0.0f, 0.0f); @@ -1484,10 +1484,10 @@ void ShapeColliderTests::runAllTests() { capsuleMissesCapsule(); capsuleTouchesCapsule(); + sphereMissesAACube(); sphereTouchesAACubeFaces(); sphereTouchesAACubeEdges(); sphereTouchesAACubeCorners(); - sphereMissesAACube(); rayHitsSphere(); rayBarelyHitsSphere(); From 921c8cfec37db5211dd1c04aba4e536cb16a914a Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 27 Aug 2014 12:21:15 -0700 Subject: [PATCH 5/9] fix broken build for using unititialized const float --- libraries/shared/src/ShapeCollider.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index 0a5d167ff7..e4e769b1c7 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -624,6 +624,8 @@ 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); @@ -699,7 +701,7 @@ bool capsuleVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& co if (depth > 0.0f) { if (depth < capsuleRadius) { // need to recast against the diagonal plane: wall rotated away from the face - glm::vec3 diagonalNormal = INV_SQUARE_ROOT_OF_2 * (wallNormal - faceNormal); + 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 From 00913d4422318d22d060f602ea3bbc92b12b94a2 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 28 Aug 2014 16:51:16 -0700 Subject: [PATCH 6/9] 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(); From a0eb13f6db573a2c7e6de6681ff89cc861abe59e Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 29 Aug 2014 14:23:00 -0700 Subject: [PATCH 7/9] collision logic for capsule-side-vs-cube-face with unit tests --- libraries/shared/src/ShapeCollider.cpp | 110 ++++++++- tests/physics/src/ShapeColliderTests.cpp | 281 ++++++++++++++++++++--- 2 files changed, 360 insertions(+), 31 deletions(-) diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index 7aaf71c44d..c0b597e04c 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -624,6 +624,106 @@ 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 }; +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); @@ -661,10 +761,12 @@ bool capsuleVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& co } 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; + 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. diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp index ab9845ed8b..88cb9fa548 100644 --- a/tests/physics/src/ShapeColliderTests.cpp +++ b/tests/physics/src/ShapeColliderTests.cpp @@ -832,7 +832,8 @@ void ShapeColliderTests::sphereTouchesAACubeFaces() { cubeCenter, cubeSide, collisions); if (!collision) { - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube. faceNormal = " << faceNormal + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide outside cube face." + << " faceNormal = " << faceNormal << std::endl; break; } @@ -868,7 +869,7 @@ void ShapeColliderTests::sphereTouchesAACubeFaces() { cubeCenter, cubeSide, collisions); if (!collision) { - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube." + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide inside cube face." << " faceNormal = " << faceNormal << std::endl; break; } @@ -1037,11 +1038,10 @@ void ShapeColliderTests::capsuleMissesAACube() { float offset = 2.0f * EPSILON; - // faces + // 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]; @@ -1066,15 +1066,14 @@ void ShapeColliderTests::capsuleMissesAACube() { CapsuleShape capsule(capsuleRadius, startPoint, endPoint); // collide capsule with cube - bool hit = ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions); - if (hit) { + if (ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should NOT collide with cube face." << " faceNormal = " << faceNormal << std::endl; } } } - // edges + // 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) { @@ -1126,7 +1125,7 @@ void ShapeColliderTests::capsuleMissesAACube() { } } - // corners + // 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) { @@ -1158,14 +1157,154 @@ void ShapeColliderTests::capsuleMissesAACube() { CapsuleShape capsule(capsuleRadius, startPoint, endPoint); // collide capsule with cube - bool hit = ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions); - if (hit) { + if (ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should NOT collide with cube face." << " cornerNormal = " << cornerNormal << 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; + } + } + } + } + } + + // 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; + } + } + } + } + + // capsule sides almost hit cube faces + // these are the steps along the capsuleAxis where we'll put the capsule endpoints + float steps[] = { -1.0f, 2.0f, 0.25f, 0.75f, -1.0f }; + + for (int i = 0; i < numDirections; ++i) { + for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) { + glm::vec3 faceNormal = sign * faceNormals[i]; + glm::vec3 secondNormal = faceNormals[(i + 1) % numDirections]; + glm::vec3 thirdNormal = faceNormals[(i + 2) % numDirections]; + + // pick two random point on opposite edges of the face + glm::vec3 firstEdgeIntersection = cubeCenter + (0.5f * cubeSide) * (faceNormal + secondNormal) + + (cubeSide * (randFloat() - 0.5f)) * thirdNormal; + glm::vec3 secondEdgeIntersection = cubeCenter + (0.5f * cubeSide) * (faceNormal - secondNormal) + + (cubeSide * (randFloat() - 0.5f)) * thirdNormal; + + // compute the un-normalized axis for the capsule + glm::vec3 capsuleAxis = secondEdgeIntersection - firstEdgeIntersection; + // there are three pairs in steps[] + for (int j = 0; j < 4; j++) { + collisions.clear(); + glm::vec3 startPoint = firstEdgeIntersection + steps[j] * capsuleAxis + (capsuleRadius + offset) * faceNormal; + glm::vec3 endPoint = firstEdgeIntersection + steps[j + 1] * capsuleAxis + (capsuleRadius + offset) * faceNormal; + + // create a capsule between the points + CapsuleShape capsule(capsuleRadius, startPoint, endPoint); + + // collide capsule with cube + 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() { @@ -1173,7 +1312,6 @@ void ShapeColliderTests::capsuleTouchesAACube() { 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); @@ -1184,11 +1322,10 @@ void ShapeColliderTests::capsuleTouchesAACube() { float overlap = 0.25f * capsuleRadius; float allowableError = 10.0f * EPSILON; - // capsule caps against cube faces + // 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]; @@ -1216,8 +1353,7 @@ void ShapeColliderTests::capsuleTouchesAACube() { CapsuleShape capsule(capsuleRadius, startPoint, endPoint); // collide capsule with cube - bool hit = ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions); - if (!hit) { + if (!ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should collide with cube" << " faceNormal = " << faceNormal << std::endl; break; @@ -1254,7 +1390,7 @@ void ShapeColliderTests::capsuleTouchesAACube() { } } - // capsule caps against cube edges + // 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) { @@ -1297,8 +1433,7 @@ void ShapeColliderTests::capsuleTouchesAACube() { CapsuleShape capsule(capsuleRadius, startPoint, endPoint); // collide capsule with cube - bool hit = ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions); - if (!hit) { + if (!ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should collide with cube" << " edgeNormal = " << edgeNormal << std::endl; } @@ -1336,7 +1471,7 @@ void ShapeColliderTests::capsuleTouchesAACube() { } } - // capsule caps against cube corners + // 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) { @@ -1369,8 +1504,7 @@ void ShapeColliderTests::capsuleTouchesAACube() { CapsuleShape capsule(capsuleRadius, startPoint, endPoint); // collide capsule with cube - bool hit = ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions); - if (!hit) { + if (!ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should collide with cube" << " cornerNormal = " << cornerNormal << std::endl; } @@ -1407,7 +1541,7 @@ void ShapeColliderTests::capsuleTouchesAACube() { } } - // capsule sides against cube edges + // capsule sides hit cube edges // loop over each face... float capsuleLength = 2.0f; for (int i = 0; i < numDirections; ++i) { @@ -1454,8 +1588,7 @@ void ShapeColliderTests::capsuleTouchesAACube() { CapsuleShape capsule(capsuleRadius, startPoint, endPoint); // collide capsule with cube - bool hit = ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions); - if (!hit) { + if (!ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should collide with cube" << " edgeNormal = " << edgeNormal << std::endl; } @@ -1493,7 +1626,7 @@ void ShapeColliderTests::capsuleTouchesAACube() { } } - // capsule sides against cube corners + // 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) { @@ -1534,8 +1667,7 @@ void ShapeColliderTests::capsuleTouchesAACube() { CapsuleShape capsule(capsuleRadius, startPoint, endPoint); // collide capsule with cube - bool hit = ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions); - if (!hit) { + if (!ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should collide with cube" << " cornerNormal = " << cornerNormal << std::endl; } @@ -1571,6 +1703,101 @@ void ShapeColliderTests::capsuleTouchesAACube() { } } } + + // 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; + } + } + } + } + } } From aeb355e3daf73559a085c4eea3ea114c36906f64 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 29 Aug 2014 15:53:20 -0700 Subject: [PATCH 8/9] re-enabling legacy avatar-vs-voxel collisions so that I don't break anything when this merges with upstream --- interface/src/avatar/MyAvatar.cpp | 2 - libraries/octree/src/Octree.cpp | 6 +- libraries/octree/src/Octree.h | 5 +- libraries/shared/src/ShapeCollider.cpp | 140 +++++++++++++++++++++++++ libraries/shared/src/ShapeCollider.h | 21 ++++ 5 files changed, 164 insertions(+), 10 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 17602a68c1..e51390f9d0 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1486,7 +1486,6 @@ void MyAvatar::updateCollisionWithEnvironment(float deltaTime, float radius) { static CollisionList myCollisions(64); void MyAvatar::updateCollisionWithVoxels(float deltaTime, float radius) { - /* TODO: Andrew to reimplement this const float MAX_VOXEL_COLLISION_SPEED = 100.0f; float speed = glm::length(_velocity); if (speed > MAX_VOXEL_COLLISION_SPEED) { @@ -1593,7 +1592,6 @@ void MyAvatar::updateCollisionWithVoxels(float deltaTime, float radius) { //updateCollisionSound(myCollisions[0]->_penetration, deltaTime, VOXEL_COLLISION_FREQUENCY); } _trapDuration = isTrapped ? _trapDuration + deltaTime : 0.0f; - */ } void MyAvatar::applyHardCollision(const glm::vec3& penetration, float elasticity, float damping) { diff --git a/libraries/octree/src/Octree.cpp b/libraries/octree/src/Octree.cpp index ba76119fb7..c11d23c2ec 100644 --- a/libraries/octree/src/Octree.cpp +++ b/libraries/octree/src/Octree.cpp @@ -751,7 +751,6 @@ bool findCapsulePenetrationOp(OctreeElement* element, void* extraData) { return false; } -/* TODO: Andrew to reimplement or purge this bool findShapeCollisionsOp(OctreeElement* element, void* extraData) { ShapeArgs* args = static_cast(extraData); @@ -765,14 +764,13 @@ 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; } } return false; } -*/ bool Octree::findCapsulePenetration(const glm::vec3& start, const glm::vec3& end, float radius, glm::vec3& penetration, Octree::lockType lockType, bool* accurateResult) { @@ -811,7 +809,6 @@ bool Octree::findCapsulePenetration(const glm::vec3& start, const glm::vec3& end return args.found; } -/* TODO: Andrew to reimplement or purge this bool Octree::findShapeCollisions(const Shape* shape, CollisionList& collisions, Octree::lockType lockType, bool* accurateResult) { @@ -842,7 +839,6 @@ bool Octree::findShapeCollisions(const Shape* shape, CollisionList& collisions, } return args.found; } -*/ class GetElementEnclosingArgs { public: diff --git a/libraries/octree/src/Octree.h b/libraries/octree/src/Octree.h index 978cffd724..7ab22598ef 100644 --- a/libraries/octree/src/Octree.h +++ b/libraries/octree/src/Octree.h @@ -277,9 +277,8 @@ public: bool findCapsulePenetration(const glm::vec3& start, const glm::vec3& end, float radius, glm::vec3& penetration, Octree::lockType lockType = Octree::TryLock, bool* accurateResult = NULL); -// TODO: Andrew to reimplement or purge this -// bool findShapeCollisions(const Shape* shape, CollisionList& collisions, -// Octree::lockType = Octree::TryLock, bool* accurateResult = NULL); + bool findShapeCollisions(const Shape* shape, CollisionList& collisions, + Octree::lockType = Octree::TryLock, bool* accurateResult = NULL); OctreeElement* getElementEnclosingPoint(const glm::vec3& point, Octree::lockType lockType = Octree::TryLock, bool* accurateResult = NULL); diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index c0b597e04c..399d6e3d42 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -125,6 +125,29 @@ bool collideShapesWithShapes(const QVector& shapesA, const QVectorgetType(); + if (typeA == SPHERE_SHAPE) { + return sphereVsAACubeLegacy(static_cast(shapeA), cubeCenter, cubeSide, collisions); + } else if (typeA == CAPSULE_SHAPE) { + return capsuleVsAACubeLegacy(static_cast(shapeA), cubeCenter, cubeSide, collisions); + } else if (typeA == LIST_SHAPE) { + const ListShape* listA = static_cast(shapeA); + bool touching = false; + for (int i = 0; i < listA->size() && !collisions.isFull(); ++i) { + const Shape* subShape = listA->getSubShape(i); + int subType = subShape->getType(); + if (subType == SPHERE_SHAPE) { + touching = sphereVsAACubeLegacy(static_cast(subShape), cubeCenter, cubeSide, collisions) || touching; + } else if (subType == CAPSULE_SHAPE) { + touching = capsuleVsAACubeLegacy(static_cast(subShape), cubeCenter, cubeSide, collisions) || touching; + } + } + return touching; + } + return false; +} + bool sphereVsSphere(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { const SphereShape* sphereA = static_cast(shapeA); const SphereShape* sphereB = static_cast(shapeB); @@ -511,6 +534,103 @@ bool planeVsPlane(const Shape* shapeA, const Shape* shapeB, CollisionList& colli return false; } +// helper function +bool sphereVsAACubeLegacy(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& cubeCenter, + float cubeSide, CollisionList& collisions) { + // sphere is A + // cube is B + // BA = B - A = from center of A to center of B + float halfCubeSide = 0.5f * cubeSide; + glm::vec3 BA = cubeCenter - sphereCenter; + float distance = glm::length(BA); + if (distance > EPSILON) { + float maxBA = glm::max(glm::max(glm::abs(BA.x), glm::abs(BA.y)), glm::abs(BA.z)); + if (maxBA > halfCubeSide + sphereRadius) { + // sphere misses cube entirely + return false; + } + CollisionInfo* collision = collisions.getNewCollision(); + if (!collision) { + return false; + } + if (maxBA > halfCubeSide) { + // sphere hits cube but its center is outside cube + + // compute contact anti-pole on cube (in cube frame) + glm::vec3 cubeContact = glm::abs(BA); + if (cubeContact.x > halfCubeSide) { + cubeContact.x = halfCubeSide; + } + if (cubeContact.y > halfCubeSide) { + cubeContact.y = halfCubeSide; + } + if (cubeContact.z > halfCubeSide) { + cubeContact.z = halfCubeSide; + } + glm::vec3 signs = glm::sign(BA); + cubeContact.x *= signs.x; + cubeContact.y *= signs.y; + cubeContact.z *= signs.z; + + // compute penetration direction + glm::vec3 direction = BA - cubeContact; + float lengthDirection = glm::length(direction); + if (lengthDirection < EPSILON) { + // sphereCenter is touching cube surface, so we can't use the difference between those two + // points to compute the penetration direction. Instead we use the unitary components of + // cubeContact. + direction = cubeContact / halfCubeSide; + glm::modf(BA, direction); + lengthDirection = glm::length(direction); + } else if (lengthDirection > sphereRadius) { + collisions.deleteLastCollision(); + return false; + } + direction /= lengthDirection; + + // compute collision details + collision->_contactPoint = sphereCenter + sphereRadius * direction; + collision->_penetration = sphereRadius * direction - (BA - cubeContact); + } else { + // sphere center is inside cube + // --> push out nearest face + glm::vec3 direction; + BA /= maxBA; + glm::modf(BA, direction); + float lengthDirection = glm::length(direction); + direction /= lengthDirection; + + // compute collision details + collision->_floatData = cubeSide; + collision->_vecData = cubeCenter; + collision->_penetration = (halfCubeSide * lengthDirection + sphereRadius - maxBA * glm::dot(BA, direction)) * direction; + collision->_contactPoint = sphereCenter + sphereRadius * direction; + } + collision->_floatData = cubeSide; + collision->_vecData = cubeCenter; + collision->_shapeA = NULL; + collision->_shapeB = NULL; + return true; + } else if (sphereRadius + halfCubeSide > distance) { + // NOTE: for cocentric approximation we collide sphere and cube as two spheres which means + // this algorithm will probably be wrong when both sphere and cube are very small (both ~EPSILON) + CollisionInfo* collision = collisions.getNewCollision(); + if (collision) { + // the penetration and contactPoint are undefined, so we pick a penetration direction (-yAxis) + collision->_penetration = (sphereRadius + halfCubeSide) * glm::vec3(0.0f, -1.0f, 0.0f); + // contactPoint is on surface of A + collision->_contactPoint = sphereCenter + collision->_penetration; + + collision->_floatData = cubeSide; + collision->_vecData = cubeCenter; + collision->_shapeA = NULL; + collision->_shapeB = NULL; + return true; + } + } + return false; +} + // helper function CollisionInfo* sphereVsAACubeHelper(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) { @@ -948,6 +1068,26 @@ bool sphereAACube_StarkAngles(const glm::vec3& sphereCenter, float sphereRadius, } */ +bool sphereVsAACubeLegacy(const SphereShape* sphereA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) { + return sphereVsAACubeLegacy(sphereA->getTranslation(), sphereA->getRadius(), cubeCenter, cubeSide, 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); + float offset = glm::dot(cubeCenter - capsuleA->getTranslation(), capsuleAxis); + float halfHeight = capsuleA->getHalfHeight(); + if (offset > halfHeight) { + offset = halfHeight; + } else if (offset < -halfHeight) { + offset = -halfHeight; + } + glm::vec3 nearestApproach = capsuleA->getTranslation() + offset * capsuleAxis; + // collide nearest approach like a sphere at that point + return sphereVsAACubeLegacy(nearestApproach, capsuleA->getRadius(), cubeCenter, cubeSide, collisions); +} + bool findRayIntersectionWithShapes(const QVector shapes, const glm::vec3& rayStart, const glm::vec3& rayDirection, float& minDistance) { float hitDistance = FLT_MAX; int numShapes = shapes.size(); diff --git a/libraries/shared/src/ShapeCollider.h b/libraries/shared/src/ShapeCollider.h index 43402ed2a3..3cfec4c8a2 100644 --- a/libraries/shared/src/ShapeCollider.h +++ b/libraries/shared/src/ShapeCollider.h @@ -35,6 +35,13 @@ namespace ShapeCollider { bool collideShapeWithShapes(const Shape* shapeA, const QVector& shapes, int startIndex, CollisionList& collisions); bool collideShapesWithShapes(const QVector& shapesA, const QVector& shapesB, CollisionList& collisions); + /// \param shapeA a pointer to a shape (cannot be NULL) + /// \param cubeCenter center of cube + /// \param cubeSide lenght of side of cube + /// \param collisions[out] average collision details + /// \return true if shapeA collides with axis aligned cube + bool 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) /// \param[out] collisions where to append collision details @@ -123,6 +130,20 @@ namespace ShapeCollider { /// \return true if shapes collide bool listVsList(const Shape* listA, const Shape* listB, CollisionList& collisions); + /// \param sphereA pointer to sphere (cannot be NULL) + /// \param cubeCenter center of cube + /// \param cubeSide lenght of side of cube + /// \param[out] collisions where to append collision details + /// \return true if sphereA collides with axis aligned cube + bool 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 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 /// \param direction direction of ray From 7c04a25095f50892b452e74a6bbb96f74d4c745f Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 2 Sep 2014 16:29:20 -0700 Subject: [PATCH 9/9] fix whitespace formatting of curly braces --- libraries/shared/src/AACubeShape.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/shared/src/AACubeShape.h b/libraries/shared/src/AACubeShape.h index 7bf35d7933..96010926c7 100644 --- a/libraries/shared/src/AACubeShape.h +++ b/libraries/shared/src/AACubeShape.h @@ -16,11 +16,11 @@ class AACubeShape : public Shape { public: - AACubeShape() : Shape(AACUBE_SHAPE), _scale(1.0f) {} + 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() {} + virtual ~AACubeShape() { } float getScale() const { return _scale; } void setScale(float scale) { _scale = scale; }