From f684608d1f0149fe90c4759e9f0b92a98314002f Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 25 Aug 2014 08:24:44 -0700 Subject: [PATCH 01/12] 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 02/12] 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 03/12] 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 04/12] 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 05/12] 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 06/12] 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 07/12] 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 08/12] 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 09/12] 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; } From 95cab15864229a7a29db42876f8b9fd69b0751e8 Mon Sep 17 00:00:00 2001 From: MarcelEdward Date: Wed, 3 Sep 2014 16:34:23 +0200 Subject: [PATCH 10/12] the command: cmake . -GXcode creates a CMakeFiles direcory in the test directory, causing the creation of the xcode files to fail with an the source directory does not contain a CMakeList.txt file. This patch updates the CMakeList.txt file in the test directory to ignore that created dir. --- tests/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index dd2e6e396e..862792d8ac 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -1,5 +1,6 @@ # add the test directories file(GLOB TEST_SUBDIRS RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}/*") +list(REMOVE_ITEM TEST_SUBDIRS "CMakeFiles") foreach(DIR ${TEST_SUBDIRS}) if(IS_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/${DIR}") add_subdirectory(${DIR}) From e62834c197d7705f8e1112ba647939834be063e5 Mon Sep 17 00:00:00 2001 From: MarcelEdward Date: Wed, 3 Sep 2014 16:43:39 +0200 Subject: [PATCH 11/12] This patch updates Application::updateLocationInServer() in Application.cpp The online status is updated when the position is send to the data web, a avatar gets an offline status if the data web did not get an update in the last 30 seconds. This function had an LastLocationObject in which only send in the location info to the data web in case the location changed. Removed the last location so the position of the avatar is send to the data web every 5 seconds, so that the online status remains online if the avatar is still online and does not switch to offline when the avatar does not move. --- interface/src/Application.cpp | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 10ae4b0303..9017867399 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -3349,8 +3349,6 @@ void Application::updateLocationInServer() { if (accountManager.isLoggedIn()) { - static QJsonObject lastLocationObject; - // construct a QJsonObject given the user's current address information QJsonObject updatedLocationObject; @@ -3361,14 +3359,9 @@ void Application::updateLocationInServer() { updatedLocationObject.insert("address", addressObject); - if (updatedLocationObject != lastLocationObject) { - - accountManager.authenticatedRequest("/api/v1/users/address", QNetworkAccessManager::PutOperation, - JSONCallbackParameters(), QJsonDocument(updatedLocationObject).toJson()); - - lastLocationObject = updatedLocationObject; - } - } + accountManager.authenticatedRequest("/api/v1/users/address", QNetworkAccessManager::PutOperation, + JSONCallbackParameters(), QJsonDocument(updatedLocationObject).toJson()); + } } void Application::domainChanged(const QString& domainHostname) { From 9662dced09102a978dec67967a5ad8b994463a7e Mon Sep 17 00:00:00 2001 From: MarcelEdward Date: Wed, 3 Sep 2014 16:59:00 +0200 Subject: [PATCH 12/12] This patch updates the MyAvatar.cpp file. It adds a check MyAvatar::goToLocationFromResponse to see if the user is online, if the teleport to the avatar is done and if not a message is shown which said that the user is not online --- interface/src/avatar/MyAvatar.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index e51390f9d0..5f1b81fb14 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1897,7 +1897,12 @@ void MyAvatar::resetSize() { void MyAvatar::goToLocationFromResponse(const QJsonObject& jsonObject) { QJsonObject locationObject = jsonObject["data"].toObject()["address"].toObject(); - goToLocationFromAddress(locationObject); + bool isOnline = jsonObject["data"].toObject()["online"].toBool(); + if (isOnline ) { + goToLocationFromAddress(locationObject); + } else { + QMessageBox::warning(Application::getInstance()->getWindow(), "", "The user is not online."); + } } void MyAvatar::goToLocationFromAddress(const QJsonObject& locationObject) {