From e527f49d6563e09673e923d8b64cac6b60089b8a Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 27 Jul 2015 15:30:10 -0700 Subject: [PATCH 01/11] removing interface dependency on ShapeCollider --- interface/src/avatar/MyAvatar.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index cdd7bf2b66..42c1d15db7 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -29,7 +29,6 @@ #include #include #include -#include #include #include #include @@ -105,7 +104,6 @@ MyAvatar::MyAvatar(RigPointer rig) : _rig(rig), _prevShouldDrawHead(true) { - ShapeCollider::initDispatchTable(); for (int i = 0; i < MAX_DRIVE_KEYS; i++) { _driveKeys[i] = 0.0f; } From c7f367bfd7ac3923a05f66f0ae1942572c1c3d15 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 27 Jul 2015 15:30:37 -0700 Subject: [PATCH 02/11] removing unnecessary #include --- libraries/fbx/src/FBXReader.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/libraries/fbx/src/FBXReader.cpp b/libraries/fbx/src/FBXReader.cpp index 507e7938de..4ba47d4b46 100644 --- a/libraries/fbx/src/FBXReader.cpp +++ b/libraries/fbx/src/FBXReader.cpp @@ -27,7 +27,6 @@ #include #include #include -#include #include #include From ef5e7e9ccc2fa113a7c57597dfdb7742f32ca773 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 28 Jul 2015 15:45:48 -0700 Subject: [PATCH 03/11] remove PlaneShape dependency from GeometryUtils also add unit tests for indRayRectangleIntersection() --- libraries/shared/src/GeometryUtil.cpp | 58 +++++---- tests/shared/src/GeometryUtilTests.cpp | 160 +++++++++++++++++++++++++ tests/shared/src/GeometryUtilTests.h | 28 +++++ 3 files changed, 224 insertions(+), 22 deletions(-) create mode 100644 tests/shared/src/GeometryUtilTests.cpp create mode 100644 tests/shared/src/GeometryUtilTests.h diff --git a/libraries/shared/src/GeometryUtil.cpp b/libraries/shared/src/GeometryUtil.cpp index afb5890d05..b88166995a 100644 --- a/libraries/shared/src/GeometryUtil.cpp +++ b/libraries/shared/src/GeometryUtil.cpp @@ -11,10 +11,10 @@ #include #include +#include #include "GeometryUtil.h" #include "NumericalConstants.h" -#include "PlaneShape.h" #include "RayIntersectionInfo.h" glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const glm::vec3& start, const glm::vec3& end) { @@ -496,31 +496,45 @@ void PolygonClip::copyCleanArray(int& lengthA, glm::vec2* vertexArrayA, int& len bool findRayRectangleIntersection(const glm::vec3& origin, const glm::vec3& direction, const glm::quat& rotation, const glm::vec3& position, const glm::vec2& dimensions, float& distance) { - RayIntersectionInfo rayInfo; - rayInfo._rayStart = origin; - rayInfo._rayDirection = direction; - rayInfo._rayLength = std::numeric_limits::max(); - - PlaneShape plane; - const glm::vec3 UNROTATED_NORMAL(0.0f, 0.0f, -1.0f); glm::vec3 normal = rotation * UNROTATED_NORMAL; - plane.setNormal(normal); - plane.setPoint(position); // the position is definitely a point on our plane - bool intersects = plane.findRayIntersection(rayInfo); + bool maybeIntersects = false; + float denominator = glm::dot(normal, direction); + glm::vec3 offset = origin - position; + float normDotOffset = glm::dot(offset, normal); + float d = 0.0f; + if (fabsf(denominator) < EPSILON) { + // line is perpendicular to plane + if (fabsf(normDotOffset) < EPSILON) { + // ray starts on the plane + maybeIntersects = true; - if (intersects) { - distance = rayInfo._hitDistance; - - glm::vec3 hitPosition = origin + (distance * direction); - glm::vec3 localHitPosition = glm::inverse(rotation) * (hitPosition - position); - - glm::vec2 halfDimensions = 0.5f * dimensions; - - intersects = -halfDimensions.x <= localHitPosition.x && localHitPosition.x <= halfDimensions.x - && -halfDimensions.y <= localHitPosition.y && localHitPosition.y <= halfDimensions.y; + // compute distance to closest approach + d = - glm::dot(offset, direction); // distance to closest approach of center of rectangle + if (d < 0.0f) { + // ray points away from center of rectangle, so ray's start is the closest approach + d = 0.0f; + } + } + } else { + d = - normDotOffset / denominator; + if (d > 0.0f) { + // ray points toward plane + maybeIntersects = true; + } } - return intersects; + if (maybeIntersects) { + glm::vec3 hitPosition = origin + (d * direction); + glm::vec3 localHitPosition = glm::inverse(rotation) * (hitPosition - position); + glm::vec2 halfDimensions = 0.5f * dimensions; + if (fabsf(localHitPosition.x) < halfDimensions.x && fabsf(localHitPosition.y) < halfDimensions.y) { + // only update distance on intersection + distance = d; + return true; + } + } + + return false; } diff --git a/tests/shared/src/GeometryUtilTests.cpp b/tests/shared/src/GeometryUtilTests.cpp new file mode 100644 index 0000000000..798951e304 --- /dev/null +++ b/tests/shared/src/GeometryUtilTests.cpp @@ -0,0 +1,160 @@ +// +// GeometryUtilTests.cpp +// tests/physics/src +// +// Created by Andrew Meadows on 2015.07.27 +// Copyright 2015 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 + +#include "GeometryUtilTests.h" + +#include +#include +#include + +#include <../QTestExtensions.h> + + +QTEST_MAIN(GeometryUtilTests) + +float getErrorDifference(const float& a, const float& b) { + return fabsf(a - b); +} + +float getErrorDifference(const glm::vec3& a, const glm::vec3& b) { + return glm::distance(a, b); +} + +void GeometryUtilTests::testLocalRayRectangleIntersection() { + glm::vec3 xAxis(1.0f, 0.0f, 0.0f); + glm::vec3 yAxis(0.0f, 1.0f, 0.0f); + glm::vec3 zAxis(0.0f, 0.0f, 1.0f); + + // create a rectangle in the local frame on the XY plane with normal along -zAxis + // (this is the assumed local orientation of the rectangle in findRayRectangleIntersection()) + glm::vec2 rectDimensions(3.0f, 5.0f); + glm::vec3 rectCenter(0.0f, 0.0f, 0.0f); + glm::quat rectRotation = glm::quat(); // identity + + // create points for generating rays that hit the plane and don't + glm::vec3 rayStart(1.0f, 2.0f, 3.0f); + float delta = 0.1f; + + { // verify hit + glm::vec3 rayEnd = rectCenter + rectRotation * ((0.5f * rectDimensions.x - delta) * xAxis); + glm::vec3 rayHitDirection = glm::normalize(rayEnd - rayStart); + float expectedDistance = glm::length(rayEnd - rayStart); + + float distance = FLT_MAX; + bool hit = findRayRectangleIntersection(rayStart, rayHitDirection, rectRotation, rectCenter, rectDimensions, distance); + QCOMPARE(hit, true); + QCOMPARE_WITH_ABS_ERROR(distance, expectedDistance, EPSILON); + } + + { // verify miss + glm::vec3 rayEnd = rectCenter + rectRotation * ((0.5f * rectDimensions.y + delta) * yAxis); + glm::vec3 rayMissDirection = glm::normalize(rayEnd - rayStart); + float distance = FLT_MAX; + bool hit = findRayRectangleIntersection(rayStart, rayMissDirection, rectRotation, rectCenter, rectDimensions, distance); + QCOMPARE(hit, false); + QCOMPARE(distance, FLT_MAX); // distance should be unchanged + } + + { // hit with co-planer line + float yFraction = 0.25f; + rayStart = rectCenter + rectRotation * (rectDimensions.x * xAxis + yFraction * rectDimensions.y * yAxis); + + glm::vec3 rayEnd = rectCenter - rectRotation * (rectDimensions.x * xAxis + yFraction * rectDimensions.y * yAxis); + glm::vec3 rayHitDirection = glm::normalize(rayEnd - rayStart); + float expectedDistance = rectDimensions.x; + + float distance = FLT_MAX; + bool hit = findRayRectangleIntersection(rayStart, rayHitDirection, rectRotation, rectCenter, rectDimensions, distance); + QCOMPARE(hit, true); + QCOMPARE_WITH_ABS_ERROR(distance, expectedDistance, EPSILON); + } + + { // miss with co-planer line + float yFraction = 0.75f; + rayStart = rectCenter + rectRotation * (rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis); + + glm::vec3 rayEnd = rectCenter + rectRotation * (- rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis); + glm::vec3 rayHitDirection = glm::normalize(rayEnd - rayStart); + + float distance = FLT_MAX; + bool hit = findRayRectangleIntersection(rayStart, rayHitDirection, rectRotation, rectCenter, rectDimensions, distance); + QCOMPARE(hit, false); + QCOMPARE(distance, FLT_MAX); // distance should be unchanged + } +} + +void GeometryUtilTests::testWorldRayRectangleIntersection() { + glm::vec3 xAxis(1.0f, 0.0f, 0.0f); + glm::vec3 yAxis(0.0f, 1.0f, 0.0f); + glm::vec3 zAxis(0.0f, 0.0f, 1.0f); + + // create a rectangle in the local frame on the XY plane with normal along -zAxis + // (this is the assumed local orientation of the rectangle in findRayRectangleIntersection()) + // and then rotate and shift everything into the world frame + glm::vec2 rectDimensions(3.0f, 5.0f); + glm::vec3 rectCenter(0.7f, 0.5f, -0.3f); + glm::quat rectRotation = glm::quat(); // identity + + + // create points for generating rays that hit the plane and don't + glm::vec3 rayStart(1.0f, 2.0f, 3.0f); + float delta = 0.1f; + + { // verify hit + glm::vec3 rayEnd = rectCenter + rectRotation * ((0.5f * rectDimensions.x - delta) * xAxis); + glm::vec3 rayHitDirection = glm::normalize(rayEnd - rayStart); + float expectedDistance = glm::length(rayEnd - rayStart); + + float distance = FLT_MAX; + bool hit = findRayRectangleIntersection(rayStart, rayHitDirection, rectRotation, rectCenter, rectDimensions, distance); + QCOMPARE(hit, true); + QCOMPARE_WITH_ABS_ERROR(distance, expectedDistance, EPSILON); + } + + { // verify miss + glm::vec3 rayEnd = rectCenter + rectRotation * ((0.5f * rectDimensions.y + delta) * yAxis); + glm::vec3 rayMissDirection = glm::normalize(rayEnd - rayStart); + float distance = FLT_MAX; + bool hit = findRayRectangleIntersection(rayStart, rayMissDirection, rectRotation, rectCenter, rectDimensions, distance); + QCOMPARE(hit, false); + QCOMPARE(distance, FLT_MAX); // distance should be unchanged + } + + { // hit with co-planer line + float yFraction = 0.25f; + rayStart = rectCenter + rectRotation * (rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis); + + glm::vec3 rayEnd = rectCenter - rectRotation * (rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis); + glm::vec3 rayHitDirection = glm::normalize(rayEnd - rayStart); + float expectedDistance = rectDimensions.x; + + float distance = FLT_MAX; + bool hit = findRayRectangleIntersection(rayStart, rayHitDirection, rectRotation, rectCenter, rectDimensions, distance); + QCOMPARE(hit, true); + QCOMPARE_WITH_ABS_ERROR(distance, expectedDistance, EPSILON); + } + + { // miss with co-planer line + float yFraction = 0.75f; + rayStart = rectCenter + rectRotation * (rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis); + + glm::vec3 rayEnd = rectCenter + rectRotation * (-rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis); + glm::vec3 rayHitDirection = glm::normalize(rayEnd - rayStart); + + float distance = FLT_MAX; + bool hit = findRayRectangleIntersection(rayStart, rayHitDirection, rectRotation, rectCenter, rectDimensions, distance); + QCOMPARE(hit, false); + QCOMPARE(distance, FLT_MAX); // distance should be unchanged + } +} + diff --git a/tests/shared/src/GeometryUtilTests.h b/tests/shared/src/GeometryUtilTests.h new file mode 100644 index 0000000000..d75fce556e --- /dev/null +++ b/tests/shared/src/GeometryUtilTests.h @@ -0,0 +1,28 @@ +// +// GeometryUtilTests.h +// tests/physics/src +// +// Created by Andrew Meadows on 2014.05.30 +// 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_AngularConstraintTests_h +#define hifi_AngularConstraintTests_h + +#include +#include + +class GeometryUtilTests : public QObject { + Q_OBJECT +private slots: + void testLocalRayRectangleIntersection(); + void testWorldRayRectangleIntersection(); +}; + +float getErrorDifference(const float& a, const float& b); +float getErrorDifference(const glm::vec3& a, const glm::vec3& b); + +#endif // hifi_AngularConstraintTests_h From 0a5ada3c09dfe906031d7f2b4b3dc291f85dd409 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 29 Jul 2015 09:02:24 -0700 Subject: [PATCH 04/11] remove legacy shapes --- interface/src/avatar/Avatar.h | 3 - libraries/avatars/src/AvatarData.h | 5 - libraries/entities/src/EntityItem.h | 1 - .../entities/src/EntityScriptingInterface.h | 1 - libraries/entities/src/TextEntityItem.cpp | 47 +- libraries/entities/src/WebEntityItem.cpp | 49 +- libraries/fbx/src/OBJReader.cpp | 1 - libraries/octree/src/Octree.cpp | 8 - libraries/octree/src/OctreeElement.cpp | 1 - libraries/octree/src/OctreeElement.h | 1 - libraries/render-utils/src/Model.cpp | 1 - libraries/render-utils/src/PhysicsEntity.h | 3 - libraries/script-engine/src/ScriptEngine.cpp | 3 - libraries/shared/src/AACubeShape.cpp | 76 - libraries/shared/src/AACubeShape.h | 50 - libraries/shared/src/CapsuleShape.cpp | 218 -- libraries/shared/src/CapsuleShape.h | 62 - libraries/shared/src/CollisionInfo.cpp | 110 - libraries/shared/src/CollisionInfo.h | 101 - libraries/shared/src/GeometryUtil.cpp | 1 - libraries/shared/src/ListShape.cpp | 90 - libraries/shared/src/ListShape.h | 72 - libraries/shared/src/PlaneShape.cpp | 78 - libraries/shared/src/PlaneShape.h | 30 - libraries/shared/src/RayIntersectionInfo.h | 37 - libraries/shared/src/Shape.h | 144 -- libraries/shared/src/ShapeCollider.cpp | 1211 ----------- libraries/shared/src/ShapeCollider.h | 156 -- libraries/shared/src/SphereShape.cpp | 51 - libraries/shared/src/SphereShape.h | 63 - libraries/shared/src/StreamUtils.cpp | 32 - libraries/shared/src/StreamUtils.h | 10 - tests/physics/src/CollisionInfoTests.cpp | 70 - tests/physics/src/CollisionInfoTests.h | 29 - tests/physics/src/ShapeColliderTests.cpp | 1936 ----------------- tests/physics/src/ShapeColliderTests.h | 60 - 36 files changed, 15 insertions(+), 4796 deletions(-) delete mode 100644 libraries/shared/src/AACubeShape.cpp delete mode 100644 libraries/shared/src/AACubeShape.h delete mode 100644 libraries/shared/src/CapsuleShape.cpp delete mode 100644 libraries/shared/src/CapsuleShape.h delete mode 100644 libraries/shared/src/CollisionInfo.cpp delete mode 100644 libraries/shared/src/CollisionInfo.h delete mode 100644 libraries/shared/src/ListShape.cpp delete mode 100644 libraries/shared/src/ListShape.h delete mode 100644 libraries/shared/src/PlaneShape.cpp delete mode 100644 libraries/shared/src/PlaneShape.h delete mode 100644 libraries/shared/src/RayIntersectionInfo.h delete mode 100644 libraries/shared/src/Shape.h delete mode 100644 libraries/shared/src/ShapeCollider.cpp delete mode 100644 libraries/shared/src/ShapeCollider.h delete mode 100644 libraries/shared/src/SphereShape.cpp delete mode 100644 libraries/shared/src/SphereShape.h delete mode 100644 tests/physics/src/CollisionInfoTests.cpp delete mode 100644 tests/physics/src/CollisionInfoTests.h delete mode 100644 tests/physics/src/ShapeColliderTests.cpp delete mode 100644 tests/physics/src/ShapeColliderTests.h diff --git a/interface/src/avatar/Avatar.h b/interface/src/avatar/Avatar.h index dcf37c9a1e..ea4c95aca2 100644 --- a/interface/src/avatar/Avatar.h +++ b/interface/src/avatar/Avatar.h @@ -168,9 +168,6 @@ public: friend class AvatarManager; -signals: - void collisionWithAvatar(const QUuid& myUUID, const QUuid& theirUUID, const CollisionInfo& collision); - protected: SkeletonModel _skeletonModel; glm::vec3 _skeletonOffset; diff --git a/libraries/avatars/src/AvatarData.h b/libraries/avatars/src/AvatarData.h index 60c643eff9..919004aa84 100644 --- a/libraries/avatars/src/AvatarData.h +++ b/libraries/avatars/src/AvatarData.h @@ -46,7 +46,6 @@ typedef unsigned long long quint64; #include #include -#include #include #include #include @@ -257,10 +256,6 @@ public: const HeadData* getHeadData() const { return _headData; } const HandData* getHandData() const { return _handData; } - virtual bool findSphereCollisions(const glm::vec3& particleCenter, float particleRadius, CollisionList& collisions) { - return false; - } - bool hasIdentityChangedAfterParsing(NLPacket& packet); QByteArray identityByteArray(); diff --git a/libraries/entities/src/EntityItem.h b/libraries/entities/src/EntityItem.h index b698a5c511..57f8883cea 100644 --- a/libraries/entities/src/EntityItem.h +++ b/libraries/entities/src/EntityItem.h @@ -18,7 +18,6 @@ #include #include // for Animation, AnimationCache, and AnimationPointer classes -#include #include // for EncodeBitstreamParams class #include // for OctreeElement::AppendState #include diff --git a/libraries/entities/src/EntityScriptingInterface.h b/libraries/entities/src/EntityScriptingInterface.h index c232437757..d3710c6ccc 100644 --- a/libraries/entities/src/EntityScriptingInterface.h +++ b/libraries/entities/src/EntityScriptingInterface.h @@ -16,7 +16,6 @@ #include -#include #include #include #include diff --git a/libraries/entities/src/TextEntityItem.cpp b/libraries/entities/src/TextEntityItem.cpp index a851d025fc..3ab15ba6cb 100644 --- a/libraries/entities/src/TextEntityItem.cpp +++ b/libraries/entities/src/TextEntityItem.cpp @@ -15,7 +15,7 @@ #include #include -#include +#include #include "EntityTree.h" #include "EntityTreeElement.h" @@ -128,46 +128,13 @@ void TextEntityItem::appendSubclassData(OctreePacketData* packetData, EncodeBits } - bool TextEntityItem::findDetailedRayIntersection(const glm::vec3& origin, const glm::vec3& direction, bool& keepSearching, OctreeElement*& element, float& distance, BoxFace& face, void** intersectedObject, bool precisionPicking) const { - - RayIntersectionInfo rayInfo; - rayInfo._rayStart = origin; - rayInfo._rayDirection = direction; - rayInfo._rayLength = std::numeric_limits::max(); - - PlaneShape plane; - - const glm::vec3 UNROTATED_NORMAL(0.0f, 0.0f, -1.0f); - glm::vec3 normal = getRotation() * UNROTATED_NORMAL; - plane.setNormal(normal); - plane.setPoint(getPosition()); // the position is definitely a point on our plane - - bool intersects = plane.findRayIntersection(rayInfo); - - if (intersects) { - glm::vec3 hitAt = origin + (direction * rayInfo._hitDistance); - // now we know the point the ray hit our plane - - glm::mat4 rotation = glm::mat4_cast(getRotation()); - glm::mat4 translation = glm::translate(getPosition()); - glm::mat4 entityToWorldMatrix = translation * rotation; - glm::mat4 worldToEntityMatrix = glm::inverse(entityToWorldMatrix); - - glm::vec3 dimensions = getDimensions(); - glm::vec3 registrationPoint = getRegistrationPoint(); - glm::vec3 corner = -(dimensions * registrationPoint); - AABox entityFrameBox(corner, dimensions); - - glm::vec3 entityFrameHitAt = glm::vec3(worldToEntityMatrix * glm::vec4(hitAt, 1.0f)); - - intersects = entityFrameBox.contains(entityFrameHitAt); - } - - if (intersects) { - distance = rayInfo._hitDistance; - } - return intersects; + glm::vec3 dimensions = getDimensions(); + glm::vec2 xyDimensions(dimensions.x, dimensions.y); + glm::quat rotation = getRotation(); + glm::vec3 position = getPosition() + rotation * + (dimensions * (getRegistrationPoint() - ENTITY_ITEM_DEFAULT_REGISTRATION_POINT)); + return findRayRectangleIntersection(origin, direction, rotation, position, xyDimensions, distance); } diff --git a/libraries/entities/src/WebEntityItem.cpp b/libraries/entities/src/WebEntityItem.cpp index 2cb233bbee..c3dc757199 100644 --- a/libraries/entities/src/WebEntityItem.cpp +++ b/libraries/entities/src/WebEntityItem.cpp @@ -13,7 +13,7 @@ #include #include -#include +#include #include "EntityTree.h" #include "EntityTreeElement.h" @@ -98,50 +98,17 @@ void WebEntityItem::appendSubclassData(OctreePacketData* packetData, EncodeBitst APPEND_ENTITY_PROPERTY(PROP_SOURCE_URL, _sourceUrl); } - bool WebEntityItem::findDetailedRayIntersection(const glm::vec3& origin, const glm::vec3& direction, bool& keepSearching, OctreeElement*& element, float& distance, BoxFace& face, void** intersectedObject, bool precisionPicking) const { - - RayIntersectionInfo rayInfo; - rayInfo._rayStart = origin; - rayInfo._rayDirection = direction; - rayInfo._rayLength = std::numeric_limits::max(); - - PlaneShape plane; - - const glm::vec3 UNROTATED_NORMAL(0.0f, 0.0f, -1.0f); - glm::vec3 normal = getRotation() * UNROTATED_NORMAL; - plane.setNormal(normal); - plane.setPoint(getPosition()); // the position is definitely a point on our plane - - bool intersects = plane.findRayIntersection(rayInfo); - - if (intersects) { - glm::vec3 hitAt = origin + (direction * rayInfo._hitDistance); - // now we know the point the ray hit our plane - - glm::mat4 rotation = glm::mat4_cast(getRotation()); - glm::mat4 translation = glm::translate(getPosition()); - glm::mat4 entityToWorldMatrix = translation * rotation; - glm::mat4 worldToEntityMatrix = glm::inverse(entityToWorldMatrix); - - glm::vec3 dimensions = getDimensions(); - glm::vec3 registrationPoint = getRegistrationPoint(); - glm::vec3 corner = -(dimensions * registrationPoint); - AABox entityFrameBox(corner, dimensions); - - glm::vec3 entityFrameHitAt = glm::vec3(worldToEntityMatrix * glm::vec4(hitAt, 1.0f)); - - intersects = entityFrameBox.contains(entityFrameHitAt); - } - - if (intersects) { - distance = rayInfo._hitDistance; - } - return intersects; + glm::vec3 dimensions = getDimensions(); + glm::vec2 xyDimensions(dimensions.x, dimensions.y); + glm::quat rotation = getRotation(); + glm::vec3 position = getPosition() + rotation * + (dimensions * (getRegistrationPoint() - ENTITY_ITEM_DEFAULT_REGISTRATION_POINT)); + return findRayRectangleIntersection(origin, direction, rotation, position, xyDimensions, distance); } - + void WebEntityItem::setSourceUrl(const QString& value) { if (_sourceUrl != value) { _sourceUrl = value; diff --git a/libraries/fbx/src/OBJReader.cpp b/libraries/fbx/src/OBJReader.cpp index 35ba437745..693c1f64d3 100644 --- a/libraries/fbx/src/OBJReader.cpp +++ b/libraries/fbx/src/OBJReader.cpp @@ -23,7 +23,6 @@ #include #include "FBXReader.h" #include "OBJReader.h" -#include "Shape.h" #include "ModelFormatLogging.h" diff --git a/libraries/octree/src/Octree.cpp b/libraries/octree/src/Octree.cpp index 203ff2b072..48f5f99906 100644 --- a/libraries/octree/src/Octree.cpp +++ b/libraries/octree/src/Octree.cpp @@ -38,7 +38,6 @@ #include #include #include -#include #include #include "CoverageMap.h" @@ -791,13 +790,6 @@ public: bool found; }; -class ShapeArgs { -public: - const Shape* shape; - CollisionList& collisions; - bool found; -}; - class ContentArgs { public: AACube cube; diff --git a/libraries/octree/src/OctreeElement.cpp b/libraries/octree/src/OctreeElement.cpp index e11a11fc8e..23034e636a 100644 --- a/libraries/octree/src/OctreeElement.cpp +++ b/libraries/octree/src/OctreeElement.cpp @@ -19,7 +19,6 @@ #include #include #include -#include #include "AACube.h" #include "OctalCode.h" diff --git a/libraries/octree/src/OctreeElement.h b/libraries/octree/src/OctreeElement.h index 830655242f..d2f3f7d3aa 100644 --- a/libraries/octree/src/OctreeElement.h +++ b/libraries/octree/src/OctreeElement.h @@ -24,7 +24,6 @@ #include "ViewFrustum.h" #include "OctreeConstants.h" -class CollisionList; class EncodeBitstreamParams; class Octree; class OctreeElement; diff --git a/libraries/render-utils/src/Model.cpp b/libraries/render-utils/src/Model.cpp index c28dbf4247..d352da55ee 100644 --- a/libraries/render-utils/src/Model.cpp +++ b/libraries/render-utils/src/Model.cpp @@ -16,7 +16,6 @@ #include #include -#include #include #include #include diff --git a/libraries/render-utils/src/PhysicsEntity.h b/libraries/render-utils/src/PhysicsEntity.h index 3b527c7827..f36473af26 100644 --- a/libraries/render-utils/src/PhysicsEntity.h +++ b/libraries/render-utils/src/PhysicsEntity.h @@ -18,9 +18,6 @@ #include #include -#include -#include - class PhysicsEntity { public: diff --git a/libraries/script-engine/src/ScriptEngine.cpp b/libraries/script-engine/src/ScriptEngine.cpp index d5e727657c..cfd6cda56b 100644 --- a/libraries/script-engine/src/ScriptEngine.cpp +++ b/libraries/script-engine/src/ScriptEngine.cpp @@ -20,7 +20,6 @@ #include #include #include -#include #include #include #include @@ -366,8 +365,6 @@ void ScriptEngine::init() { // constants globalObject().setProperty("TREE_SCALE", newVariant(QVariant(TREE_SCALE))); - globalObject().setProperty("COLLISION_GROUP_ENVIRONMENT", newVariant(QVariant(COLLISION_GROUP_ENVIRONMENT))); - globalObject().setProperty("COLLISION_GROUP_AVATARS", newVariant(QVariant(COLLISION_GROUP_AVATARS))); } QScriptValue ScriptEngine::registerGlobalObject(const QString& name, QObject* object) { diff --git a/libraries/shared/src/AACubeShape.cpp b/libraries/shared/src/AACubeShape.cpp deleted file mode 100644 index 5c40b3bb64..0000000000 --- a/libraries/shared/src/AACubeShape.cpp +++ /dev/null @@ -1,76 +0,0 @@ -// -// 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 -#include - -#include "AACubeShape.h" -#include "NumericalConstants.h" // for SQUARE_ROOT_OF_3 - -glm::vec3 faceNormals[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) }; - -bool AACubeShape::findRayIntersection(RayIntersectionInfo& intersection) const { - // A = ray point - // B = cube center - glm::vec3 BA = _translation - intersection._rayStart; - - // check for ray intersection with cube's bounding sphere - // a = distance along line to closest approach to B - float a = glm::dot(intersection._rayDirection, BA); - // b2 = squared distance from cube center to point of closest approach - float b2 = glm::length2(a * intersection._rayDirection - BA); - // r = bounding radius of cube - float halfSide = 0.5f * _scale; - const float r = SQUARE_ROOT_OF_3 * halfSide; - if (b2 > r * r) { - // line doesn't hit cube's bounding sphere - return false; - } - - // check for tuncated/short ray - // maxLength = maximum possible distance between rayStart and center of cube - const float maxLength = glm::min(intersection._rayLength, intersection._hitDistance) + r; - if (a * a + b2 > maxLength * maxLength) { - // ray is not long enough to reach cube's bounding sphere - // NOTE: we don't fall in here when ray's length if FLT_MAX because maxLength^2 will be inf or nan - return false; - } - - // the trivial checks have been exhausted, so must trace to each face - bool hit = false; - for (int i = 0; i < 3; ++i) { - for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) { - glm::vec3 faceNormal = sign * faceNormals[i]; - float rayDotPlane = glm::dot(intersection._rayDirection, faceNormal); - if (glm::abs(rayDotPlane) > EPSILON) { - float distanceToFace = (halfSide + glm::dot(BA, faceNormal)) / rayDotPlane; - if (distanceToFace >= 0.0f) { - glm::vec3 point = distanceToFace * intersection._rayDirection - BA; - int j = (i + 1) % 3; - int k = (i + 2) % 3; - glm::vec3 secondNormal = faceNormals[j]; - glm::vec3 thirdNormal = faceNormals[k]; - if (glm::abs(glm::dot(point, secondNormal)) > halfSide || - glm::abs(glm::dot(point, thirdNormal)) > halfSide) { - continue; - } - if (distanceToFace < intersection._hitDistance && distanceToFace < intersection._rayLength) { - intersection._hitDistance = distanceToFace; - intersection._hitNormal = faceNormal; - intersection._hitShape = const_cast(this); - hit = true; - } - } - } - } - } - return hit; -} diff --git a/libraries/shared/src/AACubeShape.h b/libraries/shared/src/AACubeShape.h deleted file mode 100644 index da7ba9d53f..0000000000 --- a/libraries/shared/src/AACubeShape.h +++ /dev/null @@ -1,50 +0,0 @@ -// -// 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 -#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(RayIntersectionInfo& intersection) const; - - float getVolume() const { return _scale * _scale * _scale; } - virtual QDebug& dumpToDebug(QDebug& debugConext) const; - -protected: - float _scale; -}; - -inline QDebug& AACubeShape::dumpToDebug(QDebug& debugConext) const { - debugConext << "AACubeShape[ (" - << "type: " << getType() - << "position: " - << getTranslation().x << ", " << getTranslation().y << ", " << getTranslation().z - << "scale: " - << getScale() - << "]"; - - return debugConext; -} - -#endif // hifi_AACubeShape_h diff --git a/libraries/shared/src/CapsuleShape.cpp b/libraries/shared/src/CapsuleShape.cpp deleted file mode 100644 index ec71ebadbb..0000000000 --- a/libraries/shared/src/CapsuleShape.cpp +++ /dev/null @@ -1,218 +0,0 @@ -// -// CapsuleShape.cpp -// libraries/shared/src -// -// Created by Andrew Meadows on 02/20/2014. -// 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 -#include - -#include "CapsuleShape.h" -#include "GeometryUtil.h" -#include "NumericalConstants.h" - -CapsuleShape::CapsuleShape() : Shape(CAPSULE_SHAPE), _radius(0.0f), _halfHeight(0.0f) {} - -CapsuleShape::CapsuleShape(float radius, float halfHeight) : Shape(CAPSULE_SHAPE), - _radius(radius), _halfHeight(halfHeight) { - updateBoundingRadius(); -} - -CapsuleShape::CapsuleShape(float radius, float halfHeight, const glm::vec3& position, const glm::quat& rotation) : - Shape(CAPSULE_SHAPE, position, rotation), _radius(radius), _halfHeight(halfHeight) { - updateBoundingRadius(); -} - -CapsuleShape::CapsuleShape(float radius, const glm::vec3& startPoint, const glm::vec3& endPoint) : - Shape(CAPSULE_SHAPE), _radius(radius), _halfHeight(0.0f) { - setEndPoints(startPoint, endPoint); -} - -/// \param[out] startPoint is the center of start cap -void CapsuleShape::getStartPoint(glm::vec3& startPoint) const { - startPoint = _translation - _rotation * glm::vec3(0.0f, _halfHeight, 0.0f); -} - -/// \param[out] endPoint is the center of the end cap -void CapsuleShape::getEndPoint(glm::vec3& endPoint) const { - endPoint = _translation + _rotation * glm::vec3(0.0f, _halfHeight, 0.0f); -} - -void CapsuleShape::computeNormalizedAxis(glm::vec3& axis) const { - // default axis of a capsule is along the yAxis - axis = _rotation * DEFAULT_CAPSULE_AXIS; -} - -void CapsuleShape::setRadius(float radius) { - _radius = radius; - updateBoundingRadius(); -} - -void CapsuleShape::setHalfHeight(float halfHeight) { - _halfHeight = halfHeight; - updateBoundingRadius(); -} - -void CapsuleShape::setRadiusAndHalfHeight(float radius, float halfHeight) { - _radius = radius; - _halfHeight = halfHeight; - updateBoundingRadius(); -} - -void CapsuleShape::setEndPoints(const glm::vec3& startPoint, const glm::vec3& endPoint) { - glm::vec3 axis = endPoint - startPoint; - _translation = 0.5f * (endPoint + startPoint); - float height = glm::length(axis); - if (height > EPSILON) { - _halfHeight = 0.5f * height; - axis /= height; - _rotation = computeNewRotation(axis); - } - updateBoundingRadius(); -} - -// helper -bool findRayIntersectionWithCap(const glm::vec3& sphereCenter, float sphereRadius, - const glm::vec3& capsuleCenter, RayIntersectionInfo& intersection) { - float r2 = sphereRadius * sphereRadius; - - // compute closest approach (CA) - float a = glm::dot(sphereCenter - intersection._rayStart, intersection._rayDirection); // a = distance from ray-start to CA - float b2 = glm::distance2(sphereCenter, intersection._rayStart + a * intersection._rayDirection); // b2 = squared distance from sphere-center to CA - if (b2 > r2) { - // ray does not hit sphere - return false; - } - float c = sqrtf(r2 - b2); // c = distance from CA to sphere surface along intersection._rayDirection - float d2 = glm::distance2(intersection._rayStart, sphereCenter); // d2 = squared distance from sphere-center to ray-start - float distance = FLT_MAX; - if (a < 0.0f) { - // ray points away from sphere-center - if (d2 > r2) { - // ray starts outside sphere - return false; - } - // ray starts inside sphere - distance = c + a; - } else if (d2 > r2) { - // ray starts outside sphere - distance = a - c; - } else { - // ray starts inside sphere - distance = a + c; - } - if (distance > 0.0f && distance < intersection._rayLength && distance < intersection._hitDistance) { - glm::vec3 sphereCenterToHitPoint = intersection._rayStart + distance * intersection._rayDirection - sphereCenter; - if (glm::dot(sphereCenterToHitPoint, sphereCenter - capsuleCenter) >= 0.0f) { - intersection._hitDistance = distance; - intersection._hitNormal = glm::normalize(sphereCenterToHitPoint); - return true; - } - } - return false; -} - -bool CapsuleShape::findRayIntersectionWithCaps(const glm::vec3& capsuleCenter, RayIntersectionInfo& intersection) const { - glm::vec3 capCenter; - getStartPoint(capCenter); - bool hit = findRayIntersectionWithCap(capCenter, _radius, capsuleCenter, intersection); - getEndPoint(capCenter); - hit = findRayIntersectionWithCap(capCenter, _radius, capsuleCenter, intersection) || hit; - if (hit) { - intersection._hitShape = const_cast(this); - } - return hit; -} - -bool CapsuleShape::findRayIntersection(RayIntersectionInfo& intersection) const { - // ray is U, capsule is V - glm::vec3 axisV; - computeNormalizedAxis(axisV); - glm::vec3 centerV = getTranslation(); - - // first handle parallel case - float uDotV = glm::dot(axisV, intersection._rayDirection); - glm::vec3 UV = intersection._rayStart - centerV; - if (glm::abs(1.0f - glm::abs(uDotV)) < EPSILON) { - // line and cylinder are parallel - float distanceV = glm::dot(UV, intersection._rayDirection); - if (glm::length2(UV - distanceV * intersection._rayDirection) <= _radius * _radius) { - // ray is inside cylinder's radius and might intersect caps - return findRayIntersectionWithCaps(centerV, intersection); - } - return false; - } - - // Given a line with point 'U' and normalized direction 'u' and - // a cylinder with axial point 'V', radius 'r', and normalized direction 'v' - // the intersection of the two is on the line at distance 't' from 'U'. - // - // Determining the values of t reduces to solving a quadratic equation: At^2 + Bt + C = 0 - // - // where: - // - // UV = U-V - // w = u-(u.v)v - // Q = UV-(UV.v)v - // - // A = w^2 - // B = 2(w.Q) - // C = Q^2 - r^2 - - glm::vec3 w = intersection._rayDirection - uDotV * axisV; - glm::vec3 Q = UV - glm::dot(UV, axisV) * axisV; - - // we save a few multiplies by storing 2*A rather than just A - float A2 = 2.0f * glm::dot(w, w); - float B = 2.0f * glm::dot(w, Q); - - // since C is only ever used once (in the determinant) we compute it inline - float determinant = B * B - 2.0f * A2 * (glm::dot(Q, Q) - _radius * _radius); - if (determinant < 0.0f) { - return false; - } - float hitLow = (-B - sqrtf(determinant)) / A2; - float hitHigh = -(hitLow + 2.0f * B / A2); - - if (hitLow > hitHigh) { - // re-arrange so hitLow is always the smaller value - float temp = hitHigh; - hitHigh = hitLow; - hitLow = temp; - } - if (hitLow < 0.0f) { - if (hitHigh < 0.0f) { - // capsule is completely behind rayStart - return false; - } - hitLow = hitHigh; - } - - glm::vec3 p = intersection._rayStart + hitLow * intersection._rayDirection; - float d = glm::dot(p - centerV, axisV); - if (glm::abs(d) <= getHalfHeight()) { - // we definitely hit the cylinder wall - intersection._hitDistance = hitLow; - intersection._hitNormal = glm::normalize(p - centerV - d * axisV); - intersection._hitShape = const_cast(this); - return true; - } - - // ray still might hit the caps - return findRayIntersectionWithCaps(centerV, intersection); -} - -// static -glm::quat CapsuleShape::computeNewRotation(const glm::vec3& newAxis) { - float angle = glm::angle(newAxis, DEFAULT_CAPSULE_AXIS); - if (angle > EPSILON) { - glm::vec3 rotationAxis = glm::normalize(glm::cross(DEFAULT_CAPSULE_AXIS, newAxis)); - return glm::angleAxis(angle, rotationAxis); - } - return glm::quat(); -} diff --git a/libraries/shared/src/CapsuleShape.h b/libraries/shared/src/CapsuleShape.h deleted file mode 100644 index 32c09696d7..0000000000 --- a/libraries/shared/src/CapsuleShape.h +++ /dev/null @@ -1,62 +0,0 @@ -// -// CapsuleShape.h -// libraries/shared/src -// -// Created by Andrew Meadows on 02/20/2014. -// 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_CapsuleShape_h -#define hifi_CapsuleShape_h - -#include "NumericalConstants.h" -#include "Shape.h" - -// default axis of CapsuleShape is Y-axis -const glm::vec3 DEFAULT_CAPSULE_AXIS(0.0f, 1.0f, 0.0f); - - -class CapsuleShape : public Shape { -public: - CapsuleShape(); - CapsuleShape(float radius, float halfHeight); - CapsuleShape(float radius, float halfHeight, const glm::vec3& position, const glm::quat& rotation); - CapsuleShape(float radius, const glm::vec3& startPoint, const glm::vec3& endPoint); - - virtual ~CapsuleShape() {} - - float getRadius() const { return _radius; } - virtual float getHalfHeight() const { return _halfHeight; } - - /// \param[out] startPoint is the center of start cap - virtual void getStartPoint(glm::vec3& startPoint) const; - - /// \param[out] endPoint is the center of the end cap - virtual void getEndPoint(glm::vec3& endPoint) const; - - virtual void computeNormalizedAxis(glm::vec3& axis) const; - - void setRadius(float radius); - virtual void setHalfHeight(float height); - virtual void setRadiusAndHalfHeight(float radius, float height); - - /// Sets the endpoints and updates center, rotation, and halfHeight to agree. - virtual void setEndPoints(const glm::vec3& startPoint, const glm::vec3& endPoint); - - bool findRayIntersection(RayIntersectionInfo& intersection) const; - - virtual float getVolume() const { return (PI * _radius * _radius) * (1.3333333333f * _radius + getHalfHeight()); } - -protected: - bool findRayIntersectionWithCaps(const glm::vec3& capsuleCenter, RayIntersectionInfo& intersection) const; - virtual void updateBoundingRadius() { _boundingRadius = _radius + getHalfHeight(); } - static glm::quat computeNewRotation(const glm::vec3& newAxis); - - float _radius; - float _halfHeight; -}; - -#endif // hifi_CapsuleShape_h diff --git a/libraries/shared/src/CollisionInfo.cpp b/libraries/shared/src/CollisionInfo.cpp deleted file mode 100644 index 7f145efb20..0000000000 --- a/libraries/shared/src/CollisionInfo.cpp +++ /dev/null @@ -1,110 +0,0 @@ -// -// CollisionInfo.cpp -// libraries/shared/src -// -// Created by Andrew Meadows on 02/14/2014. -// 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 "CollisionInfo.h" -#include "NumericalConstants.h" -#include "Shape.h" - -CollisionInfo::CollisionInfo() : - _data(NULL), - _intData(0), - _shapeA(NULL), - _shapeB(NULL), - _damping(0.0f), - _elasticity(1.0f), - _contactPoint(0.0f), - _penetration(0.0f), - _addedVelocity(0.0f) { -} - -quint64 CollisionInfo::getShapePairKey() const { - if (_shapeB == NULL || _shapeA == NULL) { - // zero is an invalid key - return 0; - } - quint32 idA = _shapeA->getID(); - quint32 idB = _shapeB->getID(); - return idA < idB ? ((quint64)idA << 32) + (quint64)idB : ((quint64)idB << 32) + (quint64)idA; -} - -CollisionList::CollisionList(int maxSize) : - _maxSize(maxSize), - _size(0) { - _collisions.resize(_maxSize); -} - -void CollisionInfo::apply() { - assert(_shapeA); - // NOTE: Shape::computeEffectiveMass() has side effects: computes and caches partial Lagrangian coefficients - Shape* shapeA = const_cast(_shapeA); - float massA = shapeA->computeEffectiveMass(_penetration, _contactPoint); - float massB = MAX_SHAPE_MASS; - float totalMass = massA + massB; - if (_shapeB) { - Shape* shapeB = const_cast(_shapeB); - massB = shapeB->computeEffectiveMass(-_penetration, _contactPoint - _penetration); - totalMass = massA + massB; - if (totalMass < EPSILON) { - massA = massB = 1.0f; - totalMass = 2.0f; - } - // remember that _penetration points from A into B - shapeB->accumulateDelta(massA / totalMass, _penetration); - } - // NOTE: Shape::accumulateDelta() uses the coefficients from previous call to Shape::computeEffectiveMass() - // remember that _penetration points from A into B - shapeA->accumulateDelta(massB / totalMass, -_penetration); -} - -CollisionInfo* CollisionList::getNewCollision() { - // return pointer to existing CollisionInfo, or NULL of list is full - return (_size < _maxSize) ? &(_collisions[_size++]) : NULL; -} - -void CollisionList::deleteLastCollision() { - if (_size > 0) { - --_size; - } -} - -CollisionInfo* CollisionList::getCollision(int index) { - return (index > -1 && index < _size) ? &(_collisions[index]) : NULL; -} - -CollisionInfo* CollisionList::getLastCollision() { - return (_size > 0) ? &(_collisions[_size - 1]) : NULL; -} - -void CollisionList::clear() { - // we rely on the external context to properly set or clear the data members of CollisionInfos - /* - for (int i = 0; i < _size; ++i) { - // we only clear the important stuff - CollisionInfo& collision = _collisions[i]; - //collision._data = NULL; - //collision._intData = 0; - //collision._floatDAta = 0.0f; - //collision._vecData = glm::vec3(0.0f); - //collision._shapeA = NULL; - //collision._shapeB = NULL; - //collision._damping; - //collision._elasticity; - //collision._contactPoint; - //collision._penetration; - //collision._addedVelocity; - } - */ - _size = 0; -} - -CollisionInfo* CollisionList::operator[](int index) { - return (index > -1 && index < _size) ? &(_collisions[index]) : NULL; -} diff --git a/libraries/shared/src/CollisionInfo.h b/libraries/shared/src/CollisionInfo.h deleted file mode 100644 index e6daf949b3..0000000000 --- a/libraries/shared/src/CollisionInfo.h +++ /dev/null @@ -1,101 +0,0 @@ -// -// CollisionInfo.h -// libraries/shared/src -// -// Created by Andrew Meadows on 02/14/2014. -// 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_CollisionInfo_h -#define hifi_CollisionInfo_h - -#include -#include - -#include -#include - -class Shape; - -const quint32 COLLISION_GROUP_ENVIRONMENT = 1U << 0; -const quint32 COLLISION_GROUP_AVATARS = 1U << 1; -const quint32 COLLISION_GROUP_VOXELS = 1U << 2; -const quint32 VALID_COLLISION_GROUPS = 0x0f; - -// CollisionInfo contains details about the collision between two things: BodyA and BodyB. -// The assumption is that the context that analyzes the collision knows about BodyA but -// does not necessarily know about BodyB. Hence the data storred in the CollisionInfo -// is expected to be relative to BodyA (for example the penetration points from A into B). - -class CollisionInfo { -public: - CollisionInfo(); - ~CollisionInfo() {} - - // TODO: Andrew to get rid of these data members - void* _data; - int _intData; - float _floatData; - glm::vec3 _vecData; - - /// accumulates position changes for the shapes in this collision to resolve penetration - void apply(); - - Shape* getShapeA() const { return const_cast(_shapeA); } - Shape* getShapeB() const { return const_cast(_shapeB); } - - /// \return unique key for shape pair - quint64 getShapePairKey() const; - - const Shape* _shapeA; // pointer to shapeA in this collision - const Shape* _shapeB; // pointer to shapeB in this collision - - void* _extraData; // pointer to extraData for this collision, opaque to the collision info, useful for external data - - float _damping; // range [0,1] of friction coeficient - float _elasticity; // range [0,1] of energy conservation - glm::vec3 _contactPoint; // world-frame point on BodyA that is deepest into BodyB - glm::vec3 _penetration; // depth that BodyA penetrates into BodyB - glm::vec3 _addedVelocity; // velocity of BodyB -}; - -// CollisionList is intended to be a recycled container. Fill the CollisionInfo's, -// use them, and then clear them for the next frame or context. - -class CollisionList { -public: - CollisionList(int maxSize); - - /// \return pointer to next collision. NULL if list is full. - CollisionInfo* getNewCollision(); - - /// \forget about collision at the end - void deleteLastCollision(); - - /// \return pointer to collision by index. NULL if index out of bounds. - CollisionInfo* getCollision(int index); - - /// \return pointer to last collision on the list. NULL if list is empty - CollisionInfo* getLastCollision(); - - /// \return true if list is full - bool isFull() const { return _size == _maxSize; } - - /// \return number of valid collisions - int size() const { return _size; } - - /// Clear valid collisions. - void clear(); - - CollisionInfo* operator[](int index); - -private: - int _maxSize; // the container cannot get larger than this - int _size; // the current number of valid collisions in the list - QVector _collisions; -}; - -#endif // hifi_CollisionInfo_h diff --git a/libraries/shared/src/GeometryUtil.cpp b/libraries/shared/src/GeometryUtil.cpp index b88166995a..b612fe0696 100644 --- a/libraries/shared/src/GeometryUtil.cpp +++ b/libraries/shared/src/GeometryUtil.cpp @@ -15,7 +15,6 @@ #include "GeometryUtil.h" #include "NumericalConstants.h" -#include "RayIntersectionInfo.h" glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const glm::vec3& start, const glm::vec3& end) { // compute the projection of the point vector onto the segment vector diff --git a/libraries/shared/src/ListShape.cpp b/libraries/shared/src/ListShape.cpp deleted file mode 100644 index 255330c713..0000000000 --- a/libraries/shared/src/ListShape.cpp +++ /dev/null @@ -1,90 +0,0 @@ -// -// ListShape.cpp -// libraries/shared/src -// -// Created by Andrew Meadows on 02/20/2014. -// 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 "ListShape.h" - -// ListShapeEntry - -void ListShapeEntry::updateTransform(const glm::vec3& rootPosition, const glm::quat& rootRotation) { - _shape->setTranslation(rootPosition + rootRotation * _localPosition); - _shape->setRotation(_localRotation * rootRotation); -} - -// ListShape - -ListShape::~ListShape() { - clear(); -} - -void ListShape::setTranslation(const glm::vec3& position) { - _subShapeTransformsAreDirty = true; - Shape::setTranslation(position); -} - -void ListShape::setRotation(const glm::quat& rotation) { - _subShapeTransformsAreDirty = true; - Shape::setRotation(rotation); -} - -const Shape* ListShape::getSubShape(int index) const { - if (index < 0 || index > _subShapeEntries.size()) { - return NULL; - } - return _subShapeEntries[index]._shape; -} - -void ListShape::updateSubTransforms() { - if (_subShapeTransformsAreDirty) { - for (int i = 0; i < _subShapeEntries.size(); ++i) { - _subShapeEntries[i].updateTransform(_translation, _rotation); - } - _subShapeTransformsAreDirty = false; - } -} - -void ListShape::addShape(Shape* shape, const glm::vec3& localPosition, const glm::quat& localRotation) { - if (shape) { - ListShapeEntry entry; - entry._shape = shape; - entry._localPosition = localPosition; - entry._localRotation = localRotation; - _subShapeEntries.push_back(entry); - } -} - -void ListShape::setShapes(QVector& shapes) { - clear(); - _subShapeEntries.swap(shapes); - // TODO: audit our new list of entries and delete any that have null pointers - computeBoundingRadius(); -} - -void ListShape::clear() { - // the ListShape owns its subShapes, so they need to be deleted - for (int i = 0; i < _subShapeEntries.size(); ++i) { - delete _subShapeEntries[i]._shape; - } - _subShapeEntries.clear(); - setBoundingRadius(0.0f); -} - -void ListShape::computeBoundingRadius() { - float maxRadius = 0.0f; - for (int i = 0; i < _subShapeEntries.size(); ++i) { - ListShapeEntry& entry = _subShapeEntries[i]; - float radius = glm::length(entry._localPosition) + entry._shape->getBoundingRadius(); - if (radius > maxRadius) { - maxRadius = radius; - } - } - setBoundingRadius(maxRadius); -} - diff --git a/libraries/shared/src/ListShape.h b/libraries/shared/src/ListShape.h deleted file mode 100644 index 6352ef3f07..0000000000 --- a/libraries/shared/src/ListShape.h +++ /dev/null @@ -1,72 +0,0 @@ -// -// ListShape.h -// libraries/shared/src -// -// Created by Andrew Meadows on 02/20/2014. -// Copyright 2014 High Fidelity, Inc. -// -// ListShape: A collection of shapes, each with a local transform. -// -// 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_ListShape_h -#define hifi_ListShape_h - -#include - -#include -#include -#include - -#include "Shape.h" - - -class ListShapeEntry { -public: - void updateTransform(const glm::vec3& position, const glm::quat& rotation); - - Shape* _shape; - glm::vec3 _localPosition; - glm::quat _localRotation; -}; - -class ListShape : public Shape { -public: - - ListShape() : Shape(LIST_SHAPE), _subShapeEntries(), _subShapeTransformsAreDirty(false) {} - - ListShape(const glm::vec3& position, const glm::quat& rotation) : - Shape(LIST_SHAPE, position, rotation), _subShapeEntries(), _subShapeTransformsAreDirty(false) {} - - ~ListShape(); - - void setTranslation(const glm::vec3& position); - void setRotation(const glm::quat& rotation); - - const Shape* getSubShape(int index) const; - - void updateSubTransforms(); - - int size() const { return _subShapeEntries.size(); } - - void addShape(Shape* shape, const glm::vec3& localPosition, const glm::quat& localRotation); - - void setShapes(QVector& shapes); - - // TODO: either implement this or remove ListShape altogether - bool findRayIntersection(RayIntersectionInfo& intersection) const { return false; } - -protected: - void clear(); - void computeBoundingRadius(); - - QVector _subShapeEntries; - bool _subShapeTransformsAreDirty; - -private: - ListShape(const ListShape& otherList); // don't implement this -}; - -#endif // hifi_ListShape_h diff --git a/libraries/shared/src/PlaneShape.cpp b/libraries/shared/src/PlaneShape.cpp deleted file mode 100644 index cad04afa42..0000000000 --- a/libraries/shared/src/PlaneShape.cpp +++ /dev/null @@ -1,78 +0,0 @@ -// -// PlaneShape.cpp -// libraries/shared/src -// -// Created by Andrzej Kapolka on 4/10/2014. -// 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 "GLMHelpers.h" -#include "NumericalConstants.h" -#include "PlaneShape.h" - -const glm::vec3 UNROTATED_NORMAL(0.0f, 1.0f, 0.0f); - -PlaneShape::PlaneShape(const glm::vec4& coefficients) : - Shape(PLANE_SHAPE) { - - glm::vec3 normal = glm::vec3(coefficients); - _translation = -normal * coefficients.w; - - float angle = acosf(glm::dot(normal, UNROTATED_NORMAL)); - if (angle > EPSILON) { - if (angle > PI - EPSILON) { - _rotation = glm::angleAxis(PI, glm::vec3(1.0f, 0.0f, 0.0f)); - } else { - _rotation = glm::angleAxis(angle, glm::normalize(glm::cross(UNROTATED_NORMAL, normal))); - } - } -} - -glm::vec3 PlaneShape::getNormal() const { - return _rotation * UNROTATED_NORMAL; -} - -void PlaneShape::setNormal(const glm::vec3& direction) { - glm::vec3 oldTranslation = _translation; - _rotation = rotationBetween(UNROTATED_NORMAL, direction); - glm::vec3 normal = getNormal(); - _translation = glm::dot(oldTranslation, normal) * normal; -} - -void PlaneShape::setPoint(const glm::vec3& point) { - glm::vec3 normal = getNormal(); - _translation = glm::dot(point, normal) * normal; -} - -glm::vec4 PlaneShape::getCoefficients() const { - glm::vec3 normal = _rotation * UNROTATED_NORMAL; - return glm::vec4(normal.x, normal.y, normal.z, -glm::dot(normal, _translation)); -} - -bool PlaneShape::findRayIntersection(RayIntersectionInfo& intersection) const { - glm::vec3 n = getNormal(); - float denominator = glm::dot(n, intersection._rayDirection); - if (fabsf(denominator) < EPSILON) { - // line is parallel to plane - if (glm::dot(_translation - intersection._rayStart, n) < EPSILON) { - // ray starts on the plane - intersection._hitDistance = 0.0f; - intersection._hitNormal = n; - intersection._hitShape = const_cast(this); - return true; - } - } else { - float d = glm::dot(_translation - intersection._rayStart, n) / denominator; - if (d > 0.0f && d < intersection._rayLength && d < intersection._hitDistance) { - // ray points toward plane - intersection._hitDistance = d; - intersection._hitNormal = n; - intersection._hitShape = const_cast(this); - return true; - } - } - return false; -} diff --git a/libraries/shared/src/PlaneShape.h b/libraries/shared/src/PlaneShape.h deleted file mode 100644 index 8d6de326af..0000000000 --- a/libraries/shared/src/PlaneShape.h +++ /dev/null @@ -1,30 +0,0 @@ -// -// PlaneShape.h -// libraries/shared/src -// -// Created by Andrzej Kapolka on 4/9/2014. -// 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_PlaneShape_h -#define hifi_PlaneShape_h - -#include "Shape.h" - -class PlaneShape : public Shape { -public: - PlaneShape(const glm::vec4& coefficients = glm::vec4(0.0f, 1.0f, 0.0f, 0.0f)); - - glm::vec3 getNormal() const; - glm::vec4 getCoefficients() const; - - void setNormal(const glm::vec3& normal); - void setPoint(const glm::vec3& point); - - bool findRayIntersection(RayIntersectionInfo& intersection) const; -}; - -#endif // hifi_PlaneShape_h diff --git a/libraries/shared/src/RayIntersectionInfo.h b/libraries/shared/src/RayIntersectionInfo.h deleted file mode 100644 index 06ec0aceaa..0000000000 --- a/libraries/shared/src/RayIntersectionInfo.h +++ /dev/null @@ -1,37 +0,0 @@ -// -// RayIntersectionInfo.h -// libraries/physcis/src -// -// Created by Andrew Meadows 2014.09.09 -// 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_RayIntersectionInfo_h -#define hifi_RayIntersectionInfo_h - -#include - -class Shape; - -class RayIntersectionInfo { -public: - RayIntersectionInfo() : _rayStart(0.0f), _rayDirection(1.0f, 0.0f, 0.0f), _rayLength(FLT_MAX), - _hitDistance(FLT_MAX), _hitNormal(1.0f, 0.0f, 0.0f), _hitShape(NULL) { } - - glm::vec3 getIntersectionPoint() const { return _rayStart + _hitDistance * _rayDirection; } - - // input - glm::vec3 _rayStart; - glm::vec3 _rayDirection; - float _rayLength; - - // output - float _hitDistance; - glm::vec3 _hitNormal; - Shape* _hitShape; -}; - -#endif // hifi_RayIntersectionInfo_h diff --git a/libraries/shared/src/Shape.h b/libraries/shared/src/Shape.h deleted file mode 100644 index 048b8d7257..0000000000 --- a/libraries/shared/src/Shape.h +++ /dev/null @@ -1,144 +0,0 @@ -// -// Shape.h -// libraries/shared/src -// -// Created by Andrew Meadows on 2014. -// 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_Shape_h -#define hifi_Shape_h - -#include -#include -#include -#include -#include - -#include "RayIntersectionInfo.h" - -class PhysicsEntity; - -const float MAX_SHAPE_MASS = 1.0e18f; // something less than sqrt(FLT_MAX) - -// DANGER: until ShapeCollider goes away the order of these values matter. Specifically, -// UNKNOWN_SHAPE must be equal to the number of shapes that ShapeCollider actually supports. -const quint8 SPHERE_SHAPE = 0; -const quint8 CAPSULE_SHAPE = 1; -const quint8 PLANE_SHAPE = 2; -const quint8 AACUBE_SHAPE = 3; -const quint8 LIST_SHAPE = 4; -const quint8 UNKNOWN_SHAPE = 5; -const quint8 INVALID_SHAPE = 5; - -// new shapes to be supported by Bullet -const quint8 BOX_SHAPE = 7; -const quint8 CYLINDER_SHAPE = 8; - -class Shape { -public: - - typedef quint8 Type; - - static quint32 getNextID() { static quint32 nextID = 0; return ++nextID; } - - Shape() : _type(UNKNOWN_SHAPE), _owningEntity(NULL), _boundingRadius(0.0f), - _translation(0.0f), _rotation(), _mass(MAX_SHAPE_MASS) { - _id = getNextID(); - } - virtual ~Shape() { } - - Type getType() const { return _type; } - quint32 getID() const { return _id; } - - void setEntity(PhysicsEntity* entity) { _owningEntity = entity; } - PhysicsEntity* getEntity() const { return _owningEntity; } - - float getBoundingRadius() const { return _boundingRadius; } - - virtual const glm::quat& getRotation() const { return _rotation; } - virtual void setRotation(const glm::quat& rotation) { _rotation = rotation; } - - virtual void setTranslation(const glm::vec3& translation) { _translation = translation; } - virtual const glm::vec3& getTranslation() const { return _translation; } - - virtual void setMass(float mass) { _mass = mass; } - virtual float getMass() const { return _mass; } - - virtual bool findRayIntersection(RayIntersectionInfo& intersection) const = 0; - - /// \param penetration of collision - /// \param contactPoint of collision - /// \return the effective mass for the collision - /// For most shapes has side effects: computes and caches the partial Lagrangian coefficients which will be - /// used in the next accumulateDelta() call. - virtual float computeEffectiveMass(const glm::vec3& penetration, const glm::vec3& contactPoint) { return _mass; } - - /// \param relativeMassFactor the final ingredient for partial Lagrangian coefficients from computeEffectiveMass() - /// \param penetration the delta movement - virtual void accumulateDelta(float relativeMassFactor, const glm::vec3& penetration) {} - - virtual void applyAccumulatedDelta() {} - - /// \return volume of shape in cubic meters - virtual float getVolume() const { return 1.0; } - - virtual QDebug& dumpToDebug(QDebug& debugConext) const; - -protected: - // these ctors are protected (used by derived classes only) - Shape(Type type) : _type(type), _owningEntity(NULL), - _boundingRadius(0.0f), _translation(0.0f), - _rotation(), _mass(MAX_SHAPE_MASS) { - _id = getNextID(); - } - - Shape(Type type, const glm::vec3& position) : - _type(type), _owningEntity(NULL), - _boundingRadius(0.0f), _translation(position), - _rotation(), _mass(MAX_SHAPE_MASS) { - _id = getNextID(); - } - - Shape(Type type, const glm::vec3& position, const glm::quat& rotation) : _type(type), _owningEntity(NULL), - _boundingRadius(0.0f), _translation(position), - _rotation(rotation), _mass(MAX_SHAPE_MASS) { - _id = getNextID(); - } - - void setBoundingRadius(float radius) { _boundingRadius = radius; } - - Type _type; - quint32 _id; - PhysicsEntity* _owningEntity; - float _boundingRadius; - glm::vec3 _translation; - glm::quat _rotation; - float _mass; -}; - -inline QDebug& Shape::dumpToDebug(QDebug& debugConext) const { - debugConext << "Shape[ (" - << "type: " << getType() - << "position: " - << getTranslation().x << ", " << getTranslation().y << ", " << getTranslation().z - << "radius: " - << getBoundingRadius() - << "]"; - - return debugConext; -} - -inline QDebug operator<<(QDebug debug, const Shape& shape) { - return shape.dumpToDebug(debug); -} - -inline QDebug operator<<(QDebug debug, const Shape* shape) { - return shape->dumpToDebug(debug); -} - - -#endif // hifi_Shape_h diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp deleted file mode 100644 index fa86d8c0fb..0000000000 --- a/libraries/shared/src/ShapeCollider.cpp +++ /dev/null @@ -1,1211 +0,0 @@ -// -// ShapeCollider.cpp -// libraries/shared/src -// -// Created by Andrew Meadows on 02/20/2014. -// 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 - -#include "AACubeShape.h" -#include "CapsuleShape.h" -#include "GeometryUtil.h" -#include "ListShape.h" -#include "NumericalConstants.h" -#include "PlaneShape.h" -#include "ShapeCollider.h" -#include "SphereShape.h" -#include "StreamUtils.h" - - -// NOTE: -// -// * Large ListShape's are inefficient keep the lists short. -// * Collisions between lists of lists work in theory but are not recommended. - -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; -} - -// dummy dispatch for any non-implemented pairings -bool notImplemented(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - return false; -} - -// NOTE: hardcode the number of dispatchTable entries (NUM_SHAPE_TYPES ^2) -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) { - dispatchTable[i] = ¬Implemented; - } - - 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 -} - -bool collideShapes(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - return (*dispatchTable[shapeA->getType() + NUM_SHAPE_TYPES * shapeB->getType()])(shapeA, shapeB, collisions); -} - -static CollisionList tempCollisions(32); - -bool collideShapeWithShapes(const Shape* shapeA, const QVector& shapes, int startIndex, CollisionList& collisions) { - bool collided = false; - if (shapeA) { - int numShapes = shapes.size(); - for (int i = startIndex; i < numShapes; ++i) { - const Shape* shapeB = shapes.at(i); - if (!shapeB) { - continue; - } - if (collideShapes(shapeA, shapeB, collisions)) { - collided = true; - if (collisions.isFull()) { - break; - } - } - } - } - return collided; -} - -bool collideShapesWithShapes(const QVector& shapesA, const QVector& shapesB, CollisionList& collisions) { - bool collided = false; - int numShapesA = shapesA.size(); - for (int i = 0; i < numShapesA; ++i) { - Shape* shapeA = shapesA.at(i); - if (!shapeA) { - continue; - } - if (collideShapeWithShapes(shapeA, shapesB, 0, collisions)) { - collided = true; - if (collisions.isFull()) { - break; - } - } - } - return collided; -} - -bool collideShapeWithAACubeLegacy(const Shape* shapeA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) { - Shape::Type typeA = shapeA->getType(); - 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); - glm::vec3 BA = sphereB->getTranslation() - sphereA->getTranslation(); - float distanceSquared = glm::dot(BA, BA); - float totalRadius = sphereA->getRadius() + sphereB->getRadius(); - if (distanceSquared < totalRadius * totalRadius) { - // normalize BA - float distance = sqrtf(distanceSquared); - if (distance < EPSILON) { - // the spheres are on top of each other, so we pick an arbitrary penetration direction - BA = glm::vec3(0.0f, 1.0f, 0.0f); - distance = totalRadius; - } else { - BA /= distance; - } - // penetration points from A into B - CollisionInfo* collision = collisions.getNewCollision(); - if (collision) { - collision->_penetration = BA * (totalRadius - distance); - // contactPoint is on surface of A - collision->_contactPoint = sphereA->getTranslation() + sphereA->getRadius() * BA; - collision->_shapeA = shapeA; - collision->_shapeB = shapeB; - return true; - } - } - return false; -} - -bool sphereVsCapsule(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - const SphereShape* sphereA = static_cast(shapeA); - const CapsuleShape* capsuleB = static_cast(shapeB); - // find sphereA's closest approach to axis of capsuleB - glm::vec3 BA = capsuleB->getTranslation() - sphereA->getTranslation(); - glm::vec3 capsuleAxis; - capsuleB->computeNormalizedAxis(capsuleAxis); - float axialDistance = - glm::dot(BA, capsuleAxis); - float absAxialDistance = fabsf(axialDistance); - float totalRadius = sphereA->getRadius() + capsuleB->getRadius(); - if (absAxialDistance < totalRadius + capsuleB->getHalfHeight()) { - glm::vec3 radialAxis = BA + axialDistance * capsuleAxis; // points from A to axis of B - float radialDistance2 = glm::length2(radialAxis); - float totalRadius2 = totalRadius * totalRadius; - if (radialDistance2 > totalRadius2) { - // sphere is too far from capsule axis - return false; - } - if (absAxialDistance > capsuleB->getHalfHeight()) { - // sphere hits capsule on a cap --> recompute radialAxis to point from spherA to cap center - float sign = (axialDistance > 0.0f) ? 1.0f : -1.0f; - radialAxis = BA + (sign * capsuleB->getHalfHeight()) * capsuleAxis; - radialDistance2 = glm::length2(radialAxis); - if (radialDistance2 > totalRadius2) { - return false; - } - } - if (radialDistance2 > EPSILON * EPSILON) { - CollisionInfo* collision = collisions.getNewCollision(); - if (!collision) { - // collisions list is full - return false; - } - // normalize the radialAxis - float radialDistance = sqrtf(radialDistance2); - radialAxis /= radialDistance; - // penetration points from A into B - 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 = shapeA; - collision->_shapeB = shapeB; - } else { - // A is on B's axis, so the penetration is undefined... - if (absAxialDistance > capsuleB->getHalfHeight()) { - // ...for the cylinder case (for now we pretend the collision doesn't exist) - return false; - } - CollisionInfo* collision = collisions.getNewCollision(); - if (!collision) { - // collisions list is full - return false; - } - // ... but still defined for the cap case - if (axialDistance < 0.0f) { - // we're hitting the start cap, so we negate the capsuleAxis - capsuleAxis *= -1; - } - // penetration points from A into B - float sign = (axialDistance > 0.0f) ? -1.0f : 1.0f; - collision->_penetration = (sign * (totalRadius + capsuleB->getHalfHeight() - absAxialDistance)) * capsuleAxis; - // contactPoint is on surface of sphereA - collision->_contactPoint = sphereA->getTranslation() + (sign * sphereA->getRadius()) * capsuleAxis; - collision->_shapeA = shapeA; - collision->_shapeB = shapeB; - } - return true; - } - return false; -} - -bool sphereVsPlane(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - const SphereShape* sphereA = static_cast(shapeA); - const PlaneShape* planeB = static_cast(shapeB); - glm::vec3 penetration; - if (findSpherePlanePenetration(sphereA->getTranslation(), sphereA->getRadius(), planeB->getCoefficients(), penetration)) { - CollisionInfo* collision = collisions.getNewCollision(); - if (!collision) { - return false; // collision list is full - } - collision->_penetration = penetration; - collision->_contactPoint = sphereA->getTranslation() + sphereA->getRadius() * glm::normalize(penetration); - collision->_shapeA = shapeA; - collision->_shapeB = shapeB; - return true; - } - return false; -} - -bool capsuleVsSphere(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - return sphereVsCapsule(shapeB, shapeA, collisions); -} - -/// \param lineP point on line -/// \param lineDir normalized direction of line -/// \param cylinderP point on cylinder axis -/// \param cylinderDir normalized direction of cylinder axis -/// \param cylinderRadius radius of cylinder -/// \param hitLow[out] distance from point on line to first intersection with cylinder -/// \param hitHigh[out] distance from point on line to second intersection with cylinder -/// \return true if line hits cylinder -bool lineCylinder(const glm::vec3& lineP, const glm::vec3& lineDir, - const glm::vec3& cylinderP, const glm::vec3& cylinderDir, float cylinderRadius, - float& hitLow, float& hitHigh) { - - // first handle parallel case - float uDotV = glm::dot(lineDir, cylinderDir); - if (fabsf(1.0f - fabsf(uDotV)) < EPSILON) { - // line and cylinder are parallel - if (glm::distance2(lineP, cylinderP) <= cylinderRadius * cylinderRadius) { - // line is inside cylinder, which we consider a hit - hitLow = 0.0f; - hitHigh = 0.0f; - return true; - } - return false; - } - - // Given a line with point 'p' and normalized direction 'u' and - // a cylinder with axial point 's', radius 'r', and normalized direction 'v' - // the intersection of the two is on the line at distance 't' from 'p'. - // - // Determining the values of t reduces to solving a quadratic equation: At^2 + Bt + C = 0 - // - // where: - // - // P = p-s - // w = u-(u.v)v - // Q = P-(P.v)v - // - // A = w^2 - // B = 2(w.Q) - // C = Q^2 - r^2 - - glm::vec3 P = lineP - cylinderP; - glm::vec3 w = lineDir - uDotV * cylinderDir; - glm::vec3 Q = P - glm::dot(P, cylinderDir) * cylinderDir; - - // we save a few multiplies by storing 2*A rather than just A - float A2 = 2.0f * glm::dot(w, w); - float B = 2.0f * glm::dot(w, Q); - - // since C is only ever used once (in the determinant) we compute it inline - float determinant = B * B - 2.0f * A2 * (glm::dot(Q, Q) - cylinderRadius * cylinderRadius); - if (determinant < 0.0f) { - return false; - } - hitLow = (-B - sqrtf(determinant)) / A2; - hitHigh = -(hitLow + 2.0f * B / A2); - - if (hitLow > hitHigh) { - // re-arrange so hitLow is always the smaller value - float temp = hitHigh; - hitHigh = hitLow; - hitLow = temp; - } - return true; -} - -bool capsuleVsCapsule(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - const CapsuleShape* capsuleA = static_cast(shapeA); - const CapsuleShape* capsuleB = static_cast(shapeB); - glm::vec3 axisA; - capsuleA->computeNormalizedAxis(axisA); - glm::vec3 axisB; - capsuleB->computeNormalizedAxis(axisB); - glm::vec3 centerA = capsuleA->getTranslation(); - glm::vec3 centerB = capsuleB->getTranslation(); - - // NOTE: The formula for closest approach between two lines is: - // d = [(B - A) . (a - (a.b)b)] / (1 - (a.b)^2) - - float aDotB = glm::dot(axisA, axisB); - float denominator = 1.0f - aDotB * aDotB; - float totalRadius = capsuleA->getRadius() + capsuleB->getRadius(); - if (denominator > EPSILON) { - // perform line-cylinder intesection test between axis of cylinderA and cylinderB with exanded radius - float hitLow = 0.0f; - float hitHigh = 0.0f; - if (!lineCylinder(centerA, axisA, centerB, axisB, totalRadius, hitLow, hitHigh)) { - return false; - } - - float halfHeightA = capsuleA->getHalfHeight(); - if (hitLow > halfHeightA || hitHigh < -halfHeightA) { - // the intersections are off the ends of capsuleA - return false; - } - - // compute nearest approach on axisA of axisB - float distanceA = glm::dot((centerB - centerA), (axisA - (aDotB) * axisB)) / denominator; - // clamp to intersection zone - if (distanceA > hitLow) { - if (distanceA > hitHigh) { - distanceA = hitHigh; - } - } else { - distanceA = hitLow; - } - // clamp to capsule segment - distanceA = glm::clamp(distanceA, -halfHeightA, halfHeightA); - - // find the closest point on capsuleB to sphere on capsuleA - float distanceB = glm::dot(centerA + distanceA * axisA - centerB, axisB); - float halfHeightB = capsuleB->getHalfHeight(); - if (fabsf(distanceB) > halfHeightB) { - // we must clamp distanceB... - distanceB = glm::clamp(distanceB, -halfHeightB, halfHeightB); - // ...and therefore must recompute distanceA - distanceA = glm::clamp(glm::dot(centerB + distanceB * axisB - centerA, axisA), -halfHeightA, halfHeightA); - } - - // collide like two spheres (do most of the math relative to B) - glm::vec3 BA = (centerB + distanceB * axisB) - (centerA + distanceA * axisA); - float distanceSquared = glm::dot(BA, BA); - if (distanceSquared < totalRadius * totalRadius) { - CollisionInfo* collision = collisions.getNewCollision(); - if (!collision) { - // collisions list is full - return false; - } - // normalize BA - float distance = sqrtf(distanceSquared); - if (distance < EPSILON) { - // the contact spheres are on top of each other, so we need to pick a penetration direction... - // try vector between the capsule centers... - BA = centerB - centerA; - distanceSquared = glm::length2(BA); - if (distanceSquared > EPSILON * EPSILON) { - distance = sqrtf(distanceSquared); - BA /= distance; - } else - { - // the capsule centers are on top of each other! - // give up on a valid penetration direction and just use the yAxis - BA = glm::vec3(0.0f, 1.0f, 0.0f); - distance = glm::max(capsuleB->getRadius(), capsuleA->getRadius()); - } - } else { - BA /= distance; - } - // penetration points from A into B - collision->_penetration = BA * (totalRadius - distance); - // contactPoint is on surface of A - collision->_contactPoint = centerA + distanceA * axisA + capsuleA->getRadius() * BA; - collision->_shapeA = shapeA; - collision->_shapeB = shapeB; - return true; - } - } else { - // capsules are approximiately parallel but might still collide - glm::vec3 BA = centerB - centerA; - float axialDistance = glm::dot(BA, axisB); - if (fabsf(axialDistance) > totalRadius + capsuleA->getHalfHeight() + capsuleB->getHalfHeight()) { - return false; - } - BA = BA - axialDistance * axisB; // BA now points from centerA to axisB (perp to axis) - float distanceSquared = glm::length2(BA); - if (distanceSquared < totalRadius * totalRadius) { - CollisionInfo* collision = collisions.getNewCollision(); - if (!collision) { - // collisions list is full - return false; - } - // We have all the info we need to compute the penetration vector... - // normalize BA - float distance = sqrtf(distanceSquared); - if (distance < EPSILON) { - // the spheres are on top of each other, so we pick an arbitrary penetration direction - BA = glm::vec3(0.0f, 1.0f, 0.0f); - } else { - BA /= distance; - } - // penetration points from A into B - collision->_penetration = BA * (totalRadius - distance); - - // However we need some more world-frame info to compute the contactPoint, - // which is on the surface of capsuleA... - // - // Find the overlapping secion of the capsules --> they collide as if there were - // two spheres at the midpoint of this overlapping section. - // So we project all endpoints to axisB, find the interior pair, - // and put A's proxy sphere on axisA at the midpoint of this section. - - // sort the projections as much as possible during calculation - float points[5]; - points[0] = -capsuleB->getHalfHeight(); - points[1] = axialDistance - capsuleA->getHalfHeight(); - points[2] = axialDistance + capsuleA->getHalfHeight(); - points[3] = capsuleB->getHalfHeight(); - - // Since there are only three comparisons to do we unroll the sort algorithm... - // and use a fifth slot as temp during swap. - if (points[1] > points[2]) { - points[4] = points[1]; - points[1] = points[2]; - points[2] = points[4]; - } - if (points[2] > points[3]) { - points[4] = points[2]; - points[2] = points[3]; - points[3] = points[4]; - } - if (points[0] > points[1]) { - points[4] = points[0]; - points[0] = points[1]; - points[1] = points[4]; - } - - // 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 = shapeA; - collision->_shapeB = shapeB; - return true; - } - } - return false; -} - -bool capsuleVsPlane(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - const CapsuleShape* capsuleA = static_cast(shapeA); - const PlaneShape* planeB = static_cast(shapeB); - glm::vec3 start, end, penetration; - capsuleA->getStartPoint(start); - capsuleA->getEndPoint(end); - glm::vec4 plane = planeB->getCoefficients(); - if (findCapsulePlanePenetration(start, end, capsuleA->getRadius(), plane, penetration)) { - CollisionInfo* collision = collisions.getNewCollision(); - if (!collision) { - return false; // collision list is full - } - 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 = shapeA; - collision->_shapeB = shapeB; - return true; - } - return false; -} - -bool planeVsSphere(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - return sphereVsPlane(shapeB, shapeA, collisions); -} - -bool planeVsCapsule(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - return capsuleVsPlane(shapeB, shapeA, collisions); -} - -bool planeVsPlane(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - // technically, planes always collide unless they're parallel and not coincident; however, that's - // not going to give us any useful information - 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) { - // sphere is A - // cube is B - // BA = B - A = from center of A to center of B - float halfCubeSide = 0.5f * cubeSide; - glm::vec3 BA = cubeCenter - sphereCenter; - float distance = glm::length(BA); - if (distance > EPSILON) { - float maxBA = glm::max(glm::max(glm::abs(BA.x), glm::abs(BA.y)), glm::abs(BA.z)); - if (maxBA > halfCubeSide + sphereRadius) { - // sphere misses cube entirely - return NULL; - } - CollisionInfo* collision = collisions.getNewCollision(); - if (!collision) { - return NULL; - } - if (maxBA > halfCubeSide) { - // sphere hits cube but its center is outside cube - - // compute contact anti-pole on cube (in cube frame) - glm::vec3 cubeContact = glm::abs(BA); - if (cubeContact.x > halfCubeSide) { - cubeContact.x = halfCubeSide; - } - if (cubeContact.y > halfCubeSide) { - cubeContact.y = halfCubeSide; - } - if (cubeContact.z > halfCubeSide) { - cubeContact.z = halfCubeSide; - } - glm::vec3 signs = glm::sign(BA); - cubeContact.x *= signs.x; - cubeContact.y *= signs.y; - cubeContact.z *= signs.z; - - // compute penetration direction - glm::vec3 direction = BA - cubeContact; - float lengthDirection = glm::length(direction); - if (lengthDirection < EPSILON) { - // sphereCenter is touching cube surface, so we can't use the difference between those two - // points to compute the penetration direction. Instead we use the unitary components of - // cubeContact. - glm::modf(cubeContact / halfCubeSide, direction); - lengthDirection = glm::length(direction); - } else if (lengthDirection > sphereRadius) { - collisions.deleteLastCollision(); - return NULL; - } - direction /= lengthDirection; - - // compute collision details - collision->_contactPoint = sphereCenter + sphereRadius * direction; - collision->_penetration = sphereRadius * direction - (BA - cubeContact); - } else { - // sphere center is inside cube - // --> push out nearest face - glm::vec3 direction; - BA /= maxBA; - glm::modf(BA, direction); - float lengthDirection = glm::length(direction); - direction /= lengthDirection; - - // compute collision details - collision->_floatData = cubeSide; - collision->_vecData = cubeCenter; - collision->_penetration = (halfCubeSide * lengthDirection + sphereRadius - maxBA * glm::dot(BA, direction)) * direction; - collision->_contactPoint = sphereCenter + sphereRadius * direction; - } - collision->_shapeA = NULL; - collision->_shapeB = NULL; - return collision; - } else if (sphereRadius + halfCubeSide > distance) { - // NOTE: for cocentric approximation we collide sphere and cube as two spheres which means - // this algorithm will probably be wrong when both sphere and cube are very small (both ~EPSILON) - CollisionInfo* collision = collisions.getNewCollision(); - if (collision) { - // the penetration and contactPoint are undefined, so we pick a penetration direction (-yAxis) - collision->_penetration = (sphereRadius + halfCubeSide) * glm::vec3(0.0f, -1.0f, 0.0f); - // contactPoint is on surface of A - collision->_contactPoint = sphereCenter + collision->_penetration; - collision->_shapeA = NULL; - collision->_shapeB = NULL; - return collision; - } - } - return NULL; -} - -bool sphereVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - // BA = B - A = from center of A to center of B - const SphereShape* sphereA = static_cast(shapeA); - const AACubeShape* cubeB = static_cast(shapeB); - - CollisionInfo* collision = sphereVsAACubeHelper( sphereA->getTranslation(), sphereA->getRadius(), - cubeB->getTranslation(), cubeB->getScale(), collisions); - if (collision) { - collision->_shapeA = shapeA; - collision->_shapeB = shapeB; - return true; - } - return false; -} - -glm::vec3 cubeNormals[3] = { glm::vec3(1.0f, 0.0f, 0.0f), glm::vec3(0.0f, 1.0f, 0.0f), glm::vec3(0.0f, 0.0f, 1.0f) }; - -// the wallIndices is a sequence of pairs that represent the OTHER directions for each cube face, -// hence the first pair is (1, 2) because the OTHER faces for xFace are (yFace, zFace) = (1, 2) -int wallIndices[] = { 1, 2, 0, 2, 0, 1 }; - -bool capsuleVsAACubeFace(const CapsuleShape* capsuleA, const AACubeShape* cubeB, int faceIndex, const glm::vec3& faceNormal, CollisionList& collisions) { - // we only fall in here when the capsuleAxis is nearly parallel to the face of a cube - glm::vec3 capsuleAxis; - capsuleA->computeNormalizedAxis(capsuleAxis); - glm::vec3 cubeCenter = cubeB->getTranslation(); - - // Revisualize the capsule as a line segment between two points. We'd like to collide the - // capsule as two spheres located at the endpoints or where the line segment hits the boundary - // of the face. - - // We raytrace forward into the four planes that neigbhor the face to find the boundary - // points of the line segment. - glm::vec3 capsuleStart; - capsuleA->getStartPoint(capsuleStart); - - // translate into cube-relative frame - capsuleStart -= cubeCenter; - float capsuleLength = 2.0f * capsuleA->getHalfHeight(); - float halfCubeSide = 0.5f * cubeB->getScale(); - float capsuleRadius = capsuleA->getRadius(); - - // preload distances with values that work for when the capsuleAxis runs parallel to neighbor face - float distances[] = {FLT_MAX, -FLT_MAX, FLT_MAX, -FLT_MAX, 0.0f}; - - // Loop over the directions that are NOT parallel to face (there are two of them). - // For each direction we'll raytrace against the positive and negative planes to find where - // the axis passes through. - for (int i = 0; i < 2; ++i) { - int wallIndex = wallIndices[2 * faceIndex + i]; - glm::vec3 wallNormal = cubeNormals[wallIndex]; - // each direction has two walls: positive and negative - float axisDotWall = glm::dot(capsuleAxis, wallNormal); - if (fabsf(axisDotWall) > EPSILON) { - // formula for distance to intersection between line (P,p) and plane (V,n) is: (V-P)*n / p*n - distances[2 * i] = (halfCubeSide - glm::dot(capsuleStart, wallNormal)) / axisDotWall; - distances[2 * i + 1] = -(halfCubeSide + glm::dot(capsuleStart, wallNormal)) / axisDotWall; - } - } - - // sort the distances from large to small - int j = 3; - while (j > 0) { - for (int i = 0; i <= j; ++i) { - if (distances[i] < distances[i+1]) { - // swap (using distances[4] as temp space) - distances[4] = distances[i]; - distances[i] = distances[i+1]; - distances[i+1] = distances[4]; - } - } - --j; - } - - // the capsule overlaps when the max of the mins is less than the min of the maxes - distances[0] = glm::min(capsuleLength, distances[1]); // maxDistance - distances[1] = glm::max(0.0f, distances[2]); // minDistance - bool hit = false; - if (distances[1] < distances[0]) { - // if we collide at all it will be at two points - for (int i = 0; i < 2; ++i) { - glm::vec3 sphereCenter = cubeCenter + capsuleStart + distances[i] * capsuleAxis; - // collide like a sphere at point0 with capsuleRadius - CollisionInfo* collision = sphereVsAACubeHelper(sphereCenter, capsuleRadius, - cubeCenter, 2.0f * halfCubeSide, collisions); - if (collision) { - // we hit! so store back pointers to the shapes - collision->_shapeA = capsuleA; - collision->_shapeB = cubeB; - hit = true; - } - } - return hit; - } else if (distances[1] < capsuleLength + capsuleRadius ) { - // we might collide at the end cap - glm::vec3 sphereCenter = cubeCenter + capsuleStart + capsuleLength * capsuleAxis; - // collide like a sphere at point0 with capsuleRadius - CollisionInfo* collision = sphereVsAACubeHelper(sphereCenter, capsuleRadius, - cubeCenter, 2.0f * halfCubeSide, collisions); - if (collision) { - // we hit! so store back pointers to the shapes - collision->_shapeA = capsuleA; - collision->_shapeB = cubeB; - hit = true; - } - } else if (distances[0] > -capsuleLength) { - // we might collide at the start cap - glm::vec3 sphereCenter = cubeCenter + capsuleStart; - // collide like a sphere at point0 with capsuleRadius - CollisionInfo* collision = sphereVsAACubeHelper(sphereCenter, capsuleRadius, - cubeCenter, 2.0f * halfCubeSide, collisions); - if (collision) { - // we hit! so store back pointers to the shapes - collision->_shapeA = capsuleA; - collision->_shapeB = cubeB; - hit = true; - } - } - return hit; -} - -bool capsuleVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - const CapsuleShape* capsuleA = static_cast(shapeA); - const AACubeShape* cubeB = static_cast(shapeB); - - // find nearest approach of capsule's line segment to cube's center - glm::vec3 capsuleAxis; - capsuleA->computeNormalizedAxis(capsuleAxis); - float halfHeight = capsuleA->getHalfHeight(); - glm::vec3 cubeCenter = cubeB->getTranslation(); - glm::vec3 capsuleCenter = capsuleA->getTranslation(); - glm::vec3 BA = cubeCenter - capsuleCenter; - float axialOffset = glm::dot(capsuleAxis, BA); - if (fabsf(axialOffset) > halfHeight) { - axialOffset = (axialOffset < 0.0f) ? -halfHeight : halfHeight; - } - glm::vec3 nearestApproach = capsuleCenter + axialOffset * capsuleAxis; - - // transform nearestApproach into cube-relative frame - nearestApproach -= cubeCenter; - - // determine the face of nearest approach - glm::vec3 signs = glm::sign(nearestApproach); - int faceIndex = 0; - glm::vec3 faceNormal(signs.x, 0.0f, 0.0f); - float maxApproach = fabsf(nearestApproach.x); - if (maxApproach < fabsf(nearestApproach.y)) { - maxApproach = fabsf(nearestApproach.y); - faceIndex = 1; - faceNormal = glm::vec3(0.0f, signs.y, 0.0f); - } - if (maxApproach < fabsf(nearestApproach.z)) { - maxApproach = fabsf(nearestApproach.z); - faceIndex = 2; - faceNormal = glm::vec3(0.0f, 0.0f, signs.z); - } - - if (fabsf(glm::dot(faceNormal, capsuleAxis)) < EPSILON) { - if (glm::dot(nearestApproach, faceNormal) > cubeB->getScale() + capsuleA->getRadius()) { - return false; - } - // we expect this case to be rare but complicated enough that we split it out - // into its own helper function - return capsuleVsAACubeFace(capsuleA, cubeB, faceIndex, faceNormal, collisions); - } - - // Revisualize the capsule as a startPoint and an axis that points toward the cube face. - // We raytrace forward into the four planes that neigbhor the face to find the furthest - // point along the capsule's axis that might hit face. - glm::vec3 capsuleStart; - if (glm::dot(capsuleAxis, faceNormal) < 0.0f) { - capsuleA->getStartPoint(capsuleStart); - } else { - // NOTE: we want dot(capsuleAxis, faceNormal) to be negative which simplifies some - // logic below, so we pretend the end is the start thereby reversing its axis. - capsuleA->getEndPoint(capsuleStart); - capsuleAxis *= -1.0f; - } - // translate into cube-relative frame - capsuleStart -= cubeCenter; - float capsuleLength = 2.0f * halfHeight; - float halfCubeSide = 0.5f * cubeB->getScale(); - float capsuleRadius = capsuleA->getRadius(); - - // Loop over the directions that are NOT parallel to face (there are two of them). - // For each direction we'll raytrace along capsuleAxis to find where the axis passes - // through the furthest face and then we'll clamp to remain on the capsule's line segment - float shortestDistance = capsuleLength; - - for (int i = 0; i < 2; ++i) { - int wallIndex = wallIndices[2 * faceIndex + i]; - // each direction has two walls: positive and negative - for (float wallSign = -1.0f; wallSign < 2.0f; wallSign += 2.0f) { - glm::vec3 wallNormal = wallSign * cubeNormals[wallIndex]; - float axisDotWall = glm::dot(capsuleAxis, wallNormal); - if (axisDotWall > EPSILON) { - // formula for distance to intersection between line (P,p) and plane (V,n) is: (V-P)*n / p*n - float newDistance = (halfCubeSide - glm::dot(capsuleStart, wallNormal)) / axisDotWall; - if (newDistance < 0.0f) { - // The wall is behind the capsule, but there is still a possibility that it collides - // against the edge so we recast against the diagonal plane beteween the two faces. - // NOTE: it is impossible for the startPoint to be in front of the diagonal plane, - // therefore we know we'll get a valid distance. - glm::vec3 thirdNormal = glm::cross(faceNormal, wallNormal); - glm::vec3 diagonalNormal = glm::normalize(glm::cross(glm::cross(capsuleAxis, thirdNormal), thirdNormal)); - newDistance = glm::dot(halfCubeSide * (faceNormal + wallNormal) - capsuleStart, diagonalNormal) / - glm::dot(capsuleAxis, diagonalNormal); - } else if (newDistance < capsuleLength) { - // The wall truncates the capsule axis, but we must check the case where the capsule - // collides with an edge/corner rather than the face. The good news is that this gives us - // an opportunity to check for an early exit case. - float heightOfImpact = glm::dot(capsuleStart + newDistance * capsuleAxis, faceNormal); - if (heightOfImpact > halfCubeSide + SQUARE_ROOT_OF_2 * capsuleRadius) { - // it is impossible for the capsule to hit the face - return false; - } else { - // recast against the diagonal plane between the two faces - glm::vec3 thirdNormal = glm::cross(faceNormal, wallNormal); - glm::vec3 diagonalNormal = glm::normalize(glm::cross(glm::cross(capsuleAxis, thirdNormal), thirdNormal)); - newDistance = glm::dot(halfCubeSide * (faceNormal + wallNormal) - capsuleStart, diagonalNormal) / - glm::dot(capsuleAxis, diagonalNormal); - } - } - if (newDistance < shortestDistance) { - shortestDistance = newDistance; - } - // there can only be one hit per direction - break; - } - } - } - - // chose the point that produces the deepest penetration against face - // and translate back into real frame - glm::vec3 sphereCenter = cubeCenter + capsuleStart + shortestDistance * capsuleAxis; - - // collide like a sphere at point0 with capsuleRadius - CollisionInfo* collision = sphereVsAACubeHelper(sphereCenter, capsuleRadius, - cubeCenter, 2.0f * halfCubeSide, collisions); - if (collision) { - // we hit! so store back pointers to the shapes - collision->_shapeA = shapeA; - collision->_shapeB = shapeB; - return true; - } - return false; -} - -bool aaCubeVsSphere(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - return sphereVsAACube(shapeB, shapeA, collisions); -} - -bool aaCubeVsCapsule(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - return capsuleVsAACube(shapeB, shapeA, collisions); -} - -// helper function -CollisionInfo* aaCubeVsAACubeHelper(const glm::vec3& cubeCenterA, float cubeSideA, const glm::vec3& cubeCenterB, - float cubeSideB, CollisionList& collisions) { - // cube is A - // cube is B - // BA = B - A = from center of A to center of B - float halfCubeSideA = 0.5f * cubeSideA; - float halfCubeSideB = 0.5f * cubeSideB; - glm::vec3 BA = cubeCenterB - cubeCenterA; - - 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 > halfCubeSideB + halfCubeSideA) { - // cube misses cube entirely - return NULL; - } - CollisionInfo* collision = collisions.getNewCollision(); - if (!collision) { - return NULL; // no more room for collisions - } - if (maxBA > halfCubeSideB) { - // cube 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 > halfCubeSideB) { - cubeContact.x = halfCubeSideB; - } - if (cubeContact.y > halfCubeSideB) { - cubeContact.y = halfCubeSideB; - } - if (cubeContact.z > halfCubeSideB) { - cubeContact.z = halfCubeSideB; - } - 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) { - // cubeCenterA is touching cube B 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. - glm::modf(cubeContact / halfCubeSideB, direction); - lengthDirection = glm::length(direction); - } else if (lengthDirection > halfCubeSideA) { - collisions.deleteLastCollision(); - return NULL; - } - direction /= lengthDirection; - - // compute collision details - collision->_contactPoint = cubeCenterA + halfCubeSideA * direction; - collision->_penetration = halfCubeSideA * direction - (BA - cubeContact); - } else { - // cube 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 = cubeSideB; - collision->_vecData = cubeCenterB; - collision->_penetration = (halfCubeSideB * lengthDirection + halfCubeSideA - maxBA * glm::dot(BA, direction)) * direction; - collision->_contactPoint = cubeCenterA + halfCubeSideA * direction; - } - collision->_shapeA = NULL; - collision->_shapeB = NULL; - return collision; - } else if (halfCubeSideA + halfCubeSideB > 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 = (halfCubeSideA + halfCubeSideB) * glm::vec3(0.0f, -1.0f, 0.0f); - // contactPoint is on surface of A - collision->_contactPoint = cubeCenterA + collision->_penetration; - collision->_shapeA = NULL; - collision->_shapeB = NULL; - return collision; - } - } - return NULL; -} - -bool aaCubeVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - // BA = B - A = from center of A to center of B - const AACubeShape* cubeA = static_cast(shapeA); - const AACubeShape* cubeB = static_cast(shapeB); - CollisionInfo* collision = aaCubeVsAACubeHelper( cubeA->getTranslation(), cubeA->getScale(), - cubeB->getTranslation(), cubeB->getScale(), collisions); - if (collision) { - collision->_shapeA = shapeA; - collision->_shapeB = shapeB; - return true; - } - return false; -} - -bool shapeVsList(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - bool touching = false; - const ListShape* listB = static_cast(shapeB); - for (int i = 0; i < listB->size() && !collisions.isFull(); ++i) { - const Shape* subShape = listB->getSubShape(i); - touching = collideShapes(shapeA, subShape, collisions) || touching; - } - return touching; -} - -bool listVsShape(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - bool touching = false; - const ListShape* listA = static_cast(shapeA); - for (int i = 0; i < listA->size() && !collisions.isFull(); ++i) { - const Shape* subShape = listA->getSubShape(i); - touching = collideShapes(subShape, shapeB, collisions) || touching; - } - return touching; -} - -bool listVsList(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - bool touching = false; - const ListShape* listA = static_cast(shapeA); - const ListShape* listB = static_cast(shapeB); - for (int i = 0; i < listA->size() && !collisions.isFull(); ++i) { - const Shape* subShape = listA->getSubShape(i); - for (int j = 0; j < listB->size() && !collisions.isFull(); ++j) { - touching = collideShapes(subShape, listB->getSubShape(j), collisions) || touching; - } - } - return touching; -} - -// helper function -/* KEEP THIS CODE -- this is how to collide the cube with stark face normals (no rounding). -* We might want to use this code later for sealing boundaries between adjacent voxels. -bool sphereAACube_StarkAngles(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& cubeCenter, - float cubeSide, CollisionList& collisions) { - glm::vec3 BA = cubeCenter - sphereCenter; - float distance = glm::length(BA); - if (distance > EPSILON) { - BA /= distance; // BA is now normalized - // compute the nearest point on sphere - glm::vec3 surfaceA = sphereCenter + sphereRadius * BA; - // compute the nearest point on cube - float maxBA = glm::max(glm::max(fabsf(BA.x), fabsf(BA.y)), fabsf(BA.z)); - glm::vec3 surfaceB = cubeCenter - (0.5f * cubeSide / maxBA) * BA; - // collision happens when "vector to surfaceA from surfaceB" dots with BA to produce a positive value - glm::vec3 surfaceAB = surfaceA - surfaceB; - if (glm::dot(surfaceAB, BA) > 0.0f) { - CollisionInfo* collision = collisions.getNewCollision(); - if (collision) { - // penetration is parallel to box side direction - BA /= maxBA; - glm::vec3 direction; - glm::modf(BA, direction); - direction = glm::normalize(direction); - - // penetration is the projection of surfaceAB on direction - collision->_penetration = glm::dot(surfaceAB, direction) * direction; - // contactPoint is on surface of A - collision->_contactPoint = sphereCenter + sphereRadius * direction; - collision->_shapeA = NULL; - collision->_shapeB = NULL; - return true; - } - } - } else if (sphereRadius + 0.5f * cubeSide > 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 + 0.5f * cubeSide) * glm::vec3(0.0f, -1.0f, 0.0f); - // contactPoint is on surface of A - collision->_contactPoint = sphereCenter + collision->_penetration; - collision->_shapeA = NULL; - collision->_shapeB = NULL; - return true; - } - } - return false; -} -*/ - -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 findRayIntersection(const QVector& shapes, RayIntersectionInfo& intersection) { - int numShapes = shapes.size(); - bool hit = false; - for (int i = 0; i < numShapes; ++i) { - Shape* shape = shapes.at(i); - if (shape) { - if (shape->findRayIntersection(intersection)) { - hit = true; - } - } - } - return hit; -} - -} // namespace ShapeCollider diff --git a/libraries/shared/src/ShapeCollider.h b/libraries/shared/src/ShapeCollider.h deleted file mode 100644 index da37adcd49..0000000000 --- a/libraries/shared/src/ShapeCollider.h +++ /dev/null @@ -1,156 +0,0 @@ -// -// ShapeCollider.h -// libraries/shared/src -// -// Created by Andrew Meadows on 02/20/2014. -// 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_ShapeCollider_h -#define hifi_ShapeCollider_h - -#include - -#include "CollisionInfo.h" -#include "RayIntersectionInfo.h" -#include "SharedUtil.h" - - -class Shape; -class SphereShape; -class CapsuleShape; - -namespace ShapeCollider { - - /// MUST CALL this FIRST before using the ShapeCollider - void initDispatchTable(); - - /// \param shapeA pointer to first shape (cannot be NULL) - /// \param shapeB pointer to second shape (cannot be NULL) - /// \param collisions[out] collision details - /// \return true if shapes collide - bool collideShapes(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions); - - 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 - /// \return true if shapes collide - bool sphereVsSphere(const Shape* sphereA, const Shape* sphereB, CollisionList& collisions); - - /// \param sphereA pointer to first shape (cannot be NULL) - /// \param capsuleB pointer to second shape (cannot be NULL) - /// \param[out] collisions where to append collision details - /// \return true if shapes collide - bool sphereVsCapsule(const Shape* sphereA, const Shape* capsuleB, CollisionList& collisions); - - /// \param sphereA pointer to first shape (cannot be NULL) - /// \param planeB pointer to second shape (cannot be NULL) - /// \param[out] collisions where to append collision details - /// \return true if shapes collide - bool sphereVsPlane(const Shape* sphereA, const Shape* planeB, CollisionList& collisions); - - /// \param capsuleA pointer to first shape (cannot be NULL) - /// \param sphereB pointer to second shape (cannot be NULL) - /// \param[out] collisions where to append collision details - /// \return true if shapes collide - bool capsuleVsSphere(const Shape* capsuleA, const Shape* sphereB, CollisionList& collisions); - - /// \param capsuleA pointer to first shape (cannot be NULL) - /// \param capsuleB pointer to second shape (cannot be NULL) - /// \param[out] collisions where to append collision details - /// \return true if shapes collide - bool capsuleVsCapsule(const Shape* capsuleA, const Shape* capsuleB, CollisionList& collisions); - - /// \param capsuleA pointer to first shape (cannot be NULL) - /// \param planeB pointer to second shape (cannot be NULL) - /// \param[out] collisions where to append collision details - /// \return true if shapes collide - bool capsuleVsPlane(const Shape* capsuleA, const Shape* planeB, CollisionList& collisions); - - /// \param planeA pointer to first shape (cannot be NULL) - /// \param sphereB pointer to second shape (cannot be NULL) - /// \param[out] collisions where to append collision details - /// \return true if shapes collide - bool planeVsSphere(const Shape* planeA, const Shape* sphereB, CollisionList& collisions); - - /// \param planeA pointer to first shape (cannot be NULL) - /// \param capsuleB pointer to second shape (cannot be NULL) - /// \param[out] collisions where to append collision details - /// \return true if shapes collide - bool planeVsCapsule(const Shape* planeA, const Shape* capsuleB, CollisionList& collisions); - - /// \param planeA pointer to first shape (cannot be NULL) - /// \param planeB pointer to second shape (cannot be NULL) - /// \param[out] collisions where to append collision details - /// \return true if shapes collide - bool planeVsPlane(const Shape* planeA, const Shape* planeB, CollisionList& collisions); - - /// helper function for *VsAACube() methods - /// \param sphereCenter center of sphere - /// \param sphereRadius radius of sphere - /// \param cubeCenter center of AACube - /// \param cubeSide scale of cube - /// \param[out] collisions where to append collision details - /// \return valid pointer to CollisionInfo if sphere and cube overlap or NULL if not - CollisionInfo* sphereVsAACubeHelper(const glm::vec3& sphereCenter, float sphereRadius, - const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions); - - bool sphereVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions); - bool capsuleVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions); - bool aaCubeVsSphere(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions); - bool aaCubeVsCapsule(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions); - bool aaCubeVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions); - - /// \param shapeA pointer to first shape (cannot be NULL) - /// \param listB pointer to second shape (cannot be NULL) - /// \param[out] collisions where to append collision details - /// \return true if shapes collide - bool shapeVsList(const Shape* shapeA, const Shape* listB, CollisionList& collisions); - - /// \param listA pointer to first shape (cannot be NULL) - /// \param shapeB pointer to second shape (cannot be NULL) - /// \param[out] collisions where to append collision details - /// \return true if shapes collide - bool listVsShape(const Shape* listA, const Shape* shapeB, CollisionList& collisions); - - /// \param listA pointer to first shape (cannot be NULL) - /// \param listB pointer to second shape (cannot be NULL) - /// \param[out] collisions where to append collision details - /// \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 intersection[out] struct with info about Ray and hit - /// \return true if ray hits any shape in shapes - bool findRayIntersection(const QVector& shapes, RayIntersectionInfo& intersection); - -} // namespace ShapeCollider - -#endif // hifi_ShapeCollider_h diff --git a/libraries/shared/src/SphereShape.cpp b/libraries/shared/src/SphereShape.cpp deleted file mode 100644 index 4c47ae91c0..0000000000 --- a/libraries/shared/src/SphereShape.cpp +++ /dev/null @@ -1,51 +0,0 @@ -// -// SphereShape.cpp -// libraries/shared/src -// -// Created by Andrew Meadows on 2014.06.17 -// 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 - -#include "SphereShape.h" - -bool SphereShape::findRayIntersection(RayIntersectionInfo& intersection) const { - float r2 = _boundingRadius * _boundingRadius; - - // compute closest approach (CA) - float a = glm::dot(_translation - intersection._rayStart, intersection._rayDirection); // a = distance from ray-start to CA - float b2 = glm::distance2(_translation, intersection._rayStart + a * intersection._rayDirection); // b2 = squared distance from sphere-center to CA - if (b2 > r2) { - // ray does not hit sphere - return false; - } - float c = sqrtf(r2 - b2); // c = distance from CA to sphere surface along intersection._rayDirection - float d2 = glm::distance2(intersection._rayStart, _translation); // d2 = squared distance from sphere-center to ray-start - float distance = FLT_MAX; - if (a < 0.0f) { - // ray points away from sphere-center - if (d2 > r2) { - // ray starts outside sphere - return false; - } - // ray starts inside sphere - distance = c + a; - } else if (d2 > r2) { - // ray starts outside sphere - distance = a - c; - } else { - // ray starts inside sphere - distance = a + c; - } - if (distance > 0.0f && distance < intersection._rayLength && distance < intersection._hitDistance) { - intersection._hitDistance = distance; - intersection._hitNormal = glm::normalize(intersection._rayStart + distance * intersection._rayDirection - _translation); - intersection._hitShape = const_cast(this); - return true; - } - return false; -} diff --git a/libraries/shared/src/SphereShape.h b/libraries/shared/src/SphereShape.h deleted file mode 100644 index 964881a715..0000000000 --- a/libraries/shared/src/SphereShape.h +++ /dev/null @@ -1,63 +0,0 @@ -// -// SphereShape.h -// libraries/shared/src -// -// Created by Andrew Meadows on 2014. -// 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_SphereShape_h -#define hifi_SphereShape_h - -#include "NumericalConstants.h" -#include "Shape.h" - -class SphereShape : public Shape { -public: - SphereShape() : Shape(SPHERE_SHAPE) {} - - SphereShape(float radius) : Shape(SPHERE_SHAPE) { - _boundingRadius = radius; - } - - SphereShape(float radius, const glm::vec3& position) : Shape(SPHERE_SHAPE, position) { - _boundingRadius = radius; - } - - virtual ~SphereShape() {} - - float getRadius() const { return _boundingRadius; } - - void setRadius(float radius) { _boundingRadius = radius; } - - bool findRayIntersection(RayIntersectionInfo& intersection) const; - - float getVolume() const { return 1.3333333333f * PI * _boundingRadius * _boundingRadius * _boundingRadius; } -}; - -inline QDebug operator<<(QDebug debug, const SphereShape& shape) { - debug << "SphereShape[ (" - << "position: " - << shape.getTranslation().x << ", " << shape.getTranslation().y << ", " << shape.getTranslation().z - << "radius: " - << shape.getRadius() - << "]"; - - return debug; -} - -inline QDebug operator<<(QDebug debug, const SphereShape* shape) { - debug << "SphereShape[ (" - << "center: " - << shape->getTranslation().x << ", " << shape->getTranslation().y << ", " << shape->getTranslation().z - << "radius: " - << shape->getRadius() - << "]"; - - return debug; -} - -#endif // hifi_SphereShape_h diff --git a/libraries/shared/src/StreamUtils.cpp b/libraries/shared/src/StreamUtils.cpp index 5726728f30..d4378d82b3 100644 --- a/libraries/shared/src/StreamUtils.cpp +++ b/libraries/shared/src/StreamUtils.cpp @@ -65,38 +65,6 @@ QDataStream& operator>>(QDataStream& in, glm::quat& quaternion) { return in >> quaternion.x >> quaternion.y >> quaternion.z >> quaternion.w; } -// less common utils can be enabled with DEBUG -// FIXME, remove the second defined clause once these compile, or remove the -// functions. -#if defined(DEBUG) && defined(FIXED_STREAMS) - -std::ostream& operator<<(std::ostream& s, const CollisionInfo& c) { - s << "{penetration=" << c._penetration - << ", contactPoint=" << c._contactPoint - << ", addedVelocity=" << c._addedVelocity - << "}"; - return s; -} - -std::ostream& operator<<(std::ostream& s, const SphereShape& sphere) { - s << "{type='sphere', center=" << sphere.getPosition() - << ", radius=" << sphere.getRadius() - << "}"; - return s; -} - -std::ostream& operator<<(std::ostream& s, const CapsuleShape& capsule) { - s << "{type='capsule', center=" << capsule.getPosition() - << ", radius=" << capsule.getRadius() - << ", length=" << (2.0f * capsule.getHalfHeight()) - << ", begin=" << capsule.getStartPoint() - << ", end=" << capsule.getEndPoint() - << "}"; - return s; -} - -#endif // DEBUG - #ifndef QT_NO_DEBUG_STREAM #include diff --git a/libraries/shared/src/StreamUtils.h b/libraries/shared/src/StreamUtils.h index fd22f7c068..caf7b565f4 100644 --- a/libraries/shared/src/StreamUtils.h +++ b/libraries/shared/src/StreamUtils.h @@ -37,16 +37,6 @@ QDataStream& operator>>(QDataStream& in, glm::vec3& vector); QDataStream& operator<<(QDataStream& out, const glm::quat& quaternion); QDataStream& operator>>(QDataStream& in, glm::quat& quaternion); -// less common utils can be enabled with DEBUG -#ifdef DEBUG -class CollisionInfo; -class SphereShape; -class CapsuleShape; -std::ostream& operator<<(std::ostream& s, const CollisionInfo& c); -std::ostream& operator<<(std::ostream& s, const SphereShape& shape); -std::ostream& operator<<(std::ostream& s, const CapsuleShape& capsule); -#endif // DEBUG - #ifndef QT_NO_DEBUG_STREAM class QDebug; // Add support for writing these to qDebug(). diff --git a/tests/physics/src/CollisionInfoTests.cpp b/tests/physics/src/CollisionInfoTests.cpp deleted file mode 100644 index 70e2e14bb0..0000000000 --- a/tests/physics/src/CollisionInfoTests.cpp +++ /dev/null @@ -1,70 +0,0 @@ -// -// CollisionInfoTests.cpp -// tests/physics/src -// -// Created by Andrew Meadows on 2/21/2014. -// 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 - -#include -#include - -#include -#include -#include - -#include "CollisionInfoTests.h" - - - -QTEST_MAIN(CollisionInfoTests) -/* -static glm::vec3 xAxis(1.0f, 0.0f, 0.0f); -static glm::vec3 xZxis(0.0f, 1.0f, 0.0f); -static glm::vec3 xYxis(0.0f, 0.0f, 1.0f); - -void CollisionInfoTests::rotateThenTranslate() { - CollisionInfo collision; - collision._penetration = xAxis; - collision._contactPoint = xYxis; - collision._addedVelocity = xAxis + xYxis + xZxis; - - glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis); - float distance = 3.0f; - glm::vec3 translation = distance * xYxis; - - collision.rotateThenTranslate(rotation, translation); - QCOMPARE(collision._penetration, xYxis); - - glm::vec3 expectedContactPoint = -xAxis + translation; - QCOMPARE(collision._contactPoint, expectedContactPoint); - - glm::vec3 expectedAddedVelocity = xYxis - xAxis + xZxis; - QCOMPARE(collision._addedVelocity, expectedAddedVelocity); -} - -void CollisionInfoTests::translateThenRotate() { - CollisionInfo collision; - collision._penetration = xAxis; - collision._contactPoint = xYxis; - collision._addedVelocity = xAxis + xYxis + xZxis; - - glm::quat rotation = glm::angleAxis( -PI_OVER_TWO, zAxis); - float distance = 3.0f; - glm::vec3 translation = distance * xYxis; - - collision.translateThenRotate(translation, rotation); - QCOMPARE(collision._penetration, -xYxis); - - glm::vec3 expectedContactPoint = (1.0f + distance) * xAxis; - QCOMPARE(collision._contactPoint, expectedContactPoint); - - glm::vec3 expectedAddedVelocity = - xYxis + xAxis + xYxis; - QCOMPARE(collision._addedVelocity, expectedAddedVelocity); -}*/ - diff --git a/tests/physics/src/CollisionInfoTests.h b/tests/physics/src/CollisionInfoTests.h deleted file mode 100644 index d26d39be4b..0000000000 --- a/tests/physics/src/CollisionInfoTests.h +++ /dev/null @@ -1,29 +0,0 @@ -// -// CollisionInfoTests.h -// tests/physics/src -// -// Created by Andrew Meadows on 2/21/2014. -// 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_CollisionInfoTests_h -#define hifi_CollisionInfoTests_h - -#include - -// Add additional qtest functionality (the include order is important!) -#include "GlmTestUtils.h" -#include "../QTestExtensions.h" - -class CollisionInfoTests : public QObject { - Q_OBJECT - -private slots: -// void rotateThenTranslate(); -// void translateThenRotate(); -}; - -#endif // hifi_CollisionInfoTests_h diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp deleted file mode 100644 index cb42f534cb..0000000000 --- a/tests/physics/src/ShapeColliderTests.cpp +++ /dev/null @@ -1,1936 +0,0 @@ -// -// ShapeColliderTests.cpp -// tests/physics/src -// -// Created by Andrew Meadows on 02/21/2014. -// 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 -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "ShapeColliderTests.h" - - -const glm::vec3 origin(0.0f); -static const glm::vec3 xAxis(1.0f, 0.0f, 0.0f); -static const glm::vec3 yAxis(0.0f, 1.0f, 0.0f); -static const glm::vec3 zAxis(0.0f, 0.0f, 1.0f); - -QTEST_MAIN(ShapeColliderTests) - -void ShapeColliderTests::initTestCase() { - ShapeCollider::initDispatchTable(); -} - -void ShapeColliderTests::sphereMissesSphere() { - // non-overlapping spheres of unequal size - float radiusA = 7.0f; - float radiusB = 3.0f; - float alpha = 1.2f; - float beta = 1.3f; - glm::vec3 offsetDirection = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f)); - float offsetDistance = alpha * radiusA + beta * radiusB; - - SphereShape sphereA(radiusA, origin); - SphereShape sphereB(radiusB, offsetDistance * offsetDirection); - CollisionList collisions(16); - - // collide A to B and vice versa - QCOMPARE(ShapeCollider::collideShapes(&sphereA, &sphereB, collisions), false); - QCOMPARE(ShapeCollider::collideShapes(&sphereB, &sphereA, collisions), false); - - // Collision list should be empty - QCOMPARE(collisions.size(), 0); -} - -void ShapeColliderTests::sphereTouchesSphere() { - // overlapping spheres of unequal size - float radiusA = 7.0f; - float radiusB = 3.0f; - float alpha = 0.2f; - float beta = 0.3f; - glm::vec3 offsetDirection = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f)); - float offsetDistance = alpha * radiusA + beta * radiusB; - float expectedPenetrationDistance = (1.0f - alpha) * radiusA + (1.0f - beta) * radiusB; - glm::vec3 expectedPenetration = expectedPenetrationDistance * offsetDirection; - - SphereShape sphereA(radiusA, origin); - SphereShape sphereB(radiusB, offsetDistance * offsetDirection); - CollisionList collisions(16); - int numCollisions = 0; - - // collide A to B... - { - QCOMPARE(ShapeCollider::collideShapes(&sphereA, &sphereB, collisions), true); - ++numCollisions; - - // verify state of collisions - QCOMPARE(collisions.size(), numCollisions); - CollisionInfo* collision = collisions.getCollision(numCollisions - 1); - QVERIFY(collision != nullptr); - - // penetration points from sphereA into sphereB - QCOMPARE(collision->_penetration, expectedPenetration); - - // contactPoint is on surface of sphereA - glm::vec3 AtoB = sphereB.getTranslation() - sphereA.getTranslation(); - glm::vec3 expectedContactPoint = sphereA.getTranslation() + radiusA * glm::normalize(AtoB); - QCOMPARE(collision->_contactPoint, expectedContactPoint); - - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, EPSILON); - } - - // collide B to A... - { - QCOMPARE(ShapeCollider::collideShapes(&sphereB, &sphereA, collisions), true); - ++numCollisions; - - // penetration points from sphereA into sphereB - CollisionInfo* collision = collisions.getCollision(numCollisions - 1); - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, -expectedPenetration, EPSILON); - - // contactPoint is on surface of sphereB - glm::vec3 BtoA = sphereA.getTranslation() - sphereB.getTranslation(); - glm::vec3 expectedContactPoint = sphereB.getTranslation() + radiusB * glm::normalize(BtoA); - - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, EPSILON); - } -} - -void ShapeColliderTests::sphereMissesCapsule() { - // non-overlapping sphere and capsule - float radiusA = 1.5f; - float radiusB = 2.3f; - float totalRadius = radiusA + radiusB; - float halfHeightB = 1.7f; - float axialOffset = totalRadius + 1.1f * halfHeightB; - float radialOffset = 1.2f * radiusA + 1.3f * radiusB; - - SphereShape sphereA(radiusA); - CapsuleShape capsuleB(radiusB, halfHeightB); - - // give the capsule some arbirary transform - float angle = 37.8f; - glm::vec3 axis = glm::normalize( glm::vec3(-7.0f, 2.8f, 9.3f) ); - glm::quat rotation = glm::angleAxis(angle, axis); - glm::vec3 translation(15.1f, -27.1f, -38.6f); - capsuleB.setRotation(rotation); - capsuleB.setTranslation(translation); - - CollisionList collisions(16); - - // walk sphereA along the local yAxis next to, but not touching, capsuleB - glm::vec3 localStartPosition(radialOffset, axialOffset, 0.0f); - int numberOfSteps = 10; - float delta = 1.3f * (totalRadius + halfHeightB) / (numberOfSteps - 1); - for (int i = 0; i < numberOfSteps; ++i) { - // translate sphereA into world-frame - glm::vec3 localPosition = localStartPosition + ((float)i * delta) * yAxis; - sphereA.setTranslation(rotation * localPosition + translation); - - // sphereA agains capsuleB and vice versa - QCOMPARE(ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions), false); - QCOMPARE(ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions), false); - } - - QCOMPARE(collisions.size(), 0); -} - -void ShapeColliderTests::sphereTouchesCapsule() { - // overlapping sphere and capsule - float radiusA = 2.0f; - float radiusB = 1.0f; - float totalRadius = radiusA + radiusB; - float halfHeightB = 2.0f; - float alpha = 0.5f; - float beta = 0.5f; - float radialOffset = alpha * radiusA + beta * radiusB; - - SphereShape sphereA(radiusA); - CapsuleShape capsuleB(radiusB, halfHeightB); - - CollisionList collisions(16); - int numCollisions = 0; - - { // sphereA collides with capsuleB's cylindrical wall - sphereA.setTranslation(radialOffset * xAxis); - - QCOMPARE(ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions), true); - ++numCollisions; - - // penetration points from sphereA into capsuleB - CollisionInfo* collision = collisions.getCollision(numCollisions - 1); - glm::vec3 expectedPenetration = (radialOffset - totalRadius) * xAxis; - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, EPSILON); - - // contactPoint is on surface of sphereA - glm::vec3 expectedContactPoint = sphereA.getTranslation() - radiusA * xAxis; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, EPSILON); - - // capsuleB collides with sphereA - QCOMPARE(ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions), true); - ++numCollisions; - - // penetration points from sphereA into capsuleB - collision = collisions.getCollision(numCollisions - 1); - expectedPenetration = - (radialOffset - totalRadius) * xAxis; - if (collision->_shapeA == &sphereA) { - // the ShapeCollider swapped the order of the shapes - expectedPenetration *= -1.0f; - } - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, EPSILON); - - // contactPoint is on surface of capsuleB - glm::vec3 BtoA = sphereA.getTranslation() - capsuleB.getTranslation(); - glm::vec3 closestApproach = capsuleB.getTranslation() + glm::dot(BtoA, yAxis) * yAxis; - expectedContactPoint = closestApproach + radiusB * glm::normalize(BtoA - closestApproach); - if (collision->_shapeA == &sphereA) { - // the ShapeCollider swapped the order of the shapes - closestApproach = sphereA.getTranslation() - glm::dot(BtoA, yAxis) * yAxis; - expectedContactPoint = closestApproach - radiusB * glm::normalize(BtoA - closestApproach); - } - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, EPSILON); - } - { // sphereA hits end cap at axis - glm::vec3 axialOffset = (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis; - sphereA.setTranslation(axialOffset); - - QCOMPARE(ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions), true); - ++numCollisions; - - // penetration points from sphereA into capsuleB - CollisionInfo* collision = collisions.getCollision(numCollisions - 1); - glm::vec3 expectedPenetration = - ((1.0f - alpha) * radiusA + (1.0f - beta) * radiusB) * yAxis; - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, EPSILON); - - // contactPoint is on surface of sphereA - glm::vec3 expectedContactPoint = sphereA.getTranslation() - radiusA * yAxis; - - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, EPSILON); - - // capsuleB collides with sphereA - if (!ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions)) - { - std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: capsule and sphere should touch" << std::endl; - } else { - ++numCollisions; - } - - // penetration points from sphereA into capsuleB - collision = collisions.getCollision(numCollisions - 1); - expectedPenetration = ((1.0f - alpha) * radiusA + (1.0f - beta) * radiusB) * yAxis; - if (collision->_shapeA == &sphereA) { - // the ShapeCollider swapped the order of the shapes - expectedPenetration *= -1.0f; - } - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, EPSILON); - - // contactPoint is on surface of capsuleB - glm::vec3 endPoint; - capsuleB.getEndPoint(endPoint); - expectedContactPoint = endPoint + radiusB * yAxis; - if (collision->_shapeA == &sphereA) { - // the ShapeCollider swapped the order of the shapes - expectedContactPoint = axialOffset - radiusA * yAxis; - } - - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, EPSILON); - } - { // sphereA hits start cap at axis - glm::vec3 axialOffset = - (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis; - sphereA.setTranslation(axialOffset); - - if (!ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions)) - { - std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: sphere and capsule should touch" << std::endl; - } else { - ++numCollisions; - } - - // penetration points from sphereA into capsuleB - CollisionInfo* collision = collisions.getCollision(numCollisions - 1); - glm::vec3 expectedPenetration = ((1.0f - alpha) * radiusA + (1.0f - beta) * radiusB) * yAxis; - - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, EPSILON); - - // contactPoint is on surface of sphereA - glm::vec3 expectedContactPoint = sphereA.getTranslation() + radiusA * yAxis; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, EPSILON); - - // capsuleB collides with sphereA - if (!ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions)) - { - std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: capsule and sphere should touch" << std::endl; - } else { - ++numCollisions; - } - - // penetration points from sphereA into capsuleB - collision = collisions.getCollision(numCollisions - 1); - expectedPenetration = - ((1.0f - alpha) * radiusA + (1.0f - beta) * radiusB) * yAxis; - if (collision->_shapeA == &sphereA) { - // the ShapeCollider swapped the order of the shapes - expectedPenetration *= -1.0f; - } - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, EPSILON); - - // contactPoint is on surface of capsuleB - glm::vec3 startPoint; - capsuleB.getStartPoint(startPoint); - expectedContactPoint = startPoint - radiusB * yAxis; - if (collision->_shapeA == &sphereA) { - // the ShapeCollider swapped the order of the shapes - expectedContactPoint = axialOffset + radiusA * yAxis; - } - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, EPSILON); - } - if (collisions.size() != numCollisions) { - std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: expected " << numCollisions << " collisions but actual number is " << collisions.size() - << std::endl; - } -} - -void ShapeColliderTests::capsuleMissesCapsule() { - // non-overlapping capsules - float radiusA = 2.0f; - float halfHeightA = 3.0f; - float radiusB = 3.0f; - float halfHeightB = 4.0f; - - float totalRadius = radiusA + radiusB; - float totalHalfLength = totalRadius + halfHeightA + halfHeightB; - - CapsuleShape capsuleA(radiusA, halfHeightA); - CapsuleShape capsuleB(radiusB, halfHeightB); - - CollisionList collisions(16); - - // side by side - capsuleB.setTranslation((1.01f * totalRadius) * xAxis); - QCOMPARE(ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions), false); - QCOMPARE(ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions), false); - - // end to end - capsuleB.setTranslation((1.01f * totalHalfLength) * xAxis); - QCOMPARE(ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions), false); - QCOMPARE(ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions), false); - - // rotate B and move it to the side - glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis); - capsuleB.setRotation(rotation); - capsuleB.setTranslation((1.01f * (totalRadius + capsuleB.getHalfHeight())) * xAxis); - - QCOMPARE(ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions), false); - QCOMPARE(ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions), false); - - QCOMPARE(collisions.size(), 0); -} - -void ShapeColliderTests::capsuleTouchesCapsule() { - // overlapping capsules - float radiusA = 2.0f; - float halfHeightA = 3.0f; - float radiusB = 3.0f; - float halfHeightB = 4.0f; - - float totalRadius = radiusA + radiusB; - float totalHalfLength = totalRadius + halfHeightA + halfHeightB; - - CapsuleShape capsuleA(radiusA, halfHeightA); - CapsuleShape capsuleB(radiusB, halfHeightB); - - CollisionList collisions(16); - int numCollisions = 0; - - { // side by side - capsuleB.setTranslation((0.99f * totalRadius) * xAxis); - QCOMPARE(ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions), true); - QCOMPARE(ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions), true); - numCollisions += 2; - } - - { // end to end - capsuleB.setTranslation((0.99f * totalHalfLength) * yAxis); - - QCOMPARE(ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions), true); - QCOMPARE(ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions), true); - numCollisions += 2; - } - - { // rotate B and move it to the side - glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis); - capsuleB.setRotation(rotation); - capsuleB.setTranslation((0.99f * (totalRadius + capsuleB.getHalfHeight())) * xAxis); - - QCOMPARE(ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions), true); - QCOMPARE(ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions), true); - numCollisions += 2; - } - - { // again, but this time check collision details - float overlap = 0.1f; - glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis); - capsuleB.setRotation(rotation); - glm::vec3 positionB = ((totalRadius + capsuleB.getHalfHeight()) - overlap) * xAxis; - capsuleB.setTranslation(positionB); - - // capsuleA vs capsuleB - QCOMPARE(ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions), true); - ++numCollisions; - - CollisionInfo* collision = collisions.getCollision(numCollisions - 1); - glm::vec3 expectedPenetration = overlap * xAxis; - - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, EPSILON); - - glm::vec3 expectedContactPoint = capsuleA.getTranslation() + radiusA * xAxis; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, EPSILON); - - // capsuleB vs capsuleA - QCOMPARE(ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions), true); - ++numCollisions; - - collision = collisions.getCollision(numCollisions - 1); - expectedPenetration = - overlap * xAxis; - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, EPSILON); - - expectedContactPoint = capsuleB.getTranslation() - (radiusB + halfHeightB) * xAxis; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, EPSILON); - } - - { // collide cylinder wall against cylinder wall - float overlap = 0.137f; - float shift = 0.317f * halfHeightA; - glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis); - capsuleB.setRotation(rotation); - glm::vec3 positionB = (totalRadius - overlap) * zAxis + shift * yAxis; - capsuleB.setTranslation(positionB); - - // capsuleA vs capsuleB - QCOMPARE(ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions), true); - ++numCollisions; - - CollisionInfo* collision = collisions.getCollision(numCollisions - 1); - glm::vec3 expectedPenetration = overlap * zAxis; - - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, EPSILON); - - glm::vec3 expectedContactPoint = capsuleA.getTranslation() + radiusA * zAxis + shift * yAxis; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, EPSILON); - } -} - -void ShapeColliderTests::sphereMissesAACube() { - CollisionList collisions(16); - - float sphereRadius = 1.0f; - glm::vec3 sphereCenter(0.0f); - - glm::vec3 cubeCenter(1.5f, 0.0f, 0.0f); - - 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) { - QFAIL_WITH_MESSAGE("sphere should NOT collide with cube face.\n\t\t" - << "faceNormal = " << faceNormal); -// 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) { - QFAIL_WITH_MESSAGE("sphere should NOT collide with cube edge.\n\t\t" - << "edgeNormal = " << edgeNormal); - } - } - - } - } - } - } - - // 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) { - - QFAIL_WITH_MESSAGE("sphere should NOT collide with cube corner\n\t\t" << - "cornerNormal = " << cornerNormal); - break; - } - } - } - } - } -} - -void ShapeColliderTests::sphereTouchesAACubeFaces() { - CollisionList collisions(16); - - float sphereRadius = 1.13f; - glm::vec3 sphereCenter(0.0f); - - glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f); - float cubeSide = 4.34f; - - glm::vec3 faceNormals[] = {xAxis, yAxis, zAxis}; - int numDirections = 3; - - 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; - - // 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) { - - QFAIL_WITH_MESSAGE("sphere should collide outside cube face\n\t\t" << - "faceNormal = " << faceNormal); - break; - } - - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, EPSILON); - - glm::vec3 expectedContact = sphereCenter - sphereRadius * faceNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContact, EPSILON); - QCOMPARE(collision->getShapeA(), (Shape*)nullptr); - QCOMPARE(collision->getShapeB(), (Shape*)nullptr); - } - } - - // 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); - - if (!collision) { - QFAIL_WITH_MESSAGE("sphere should collide inside cube face.\n\t\t" - << "faceNormal = " << faceNormal); - break; - } - - glm::vec3 expectedPenetration = - overlap * faceNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, EPSILON); - - glm::vec3 expectedContact = sphereCenter - sphereRadius * faceNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContact, EPSILON); - } - } - } -} - -void ShapeColliderTests::sphereTouchesAACubeEdges() { - CollisionList collisions(20); - - float sphereRadius = 1.37f; - glm::vec3 sphereCenter(0.0f); - - glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f); - float cubeSide = 2.98f; - - float overlap = 0.25f * 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) { - QFAIL_WITH_MESSAGE("sphere should collide with cube edge.\n\t\t" - << "edgeNormal = " << edgeNormal); - break; - } - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, EPSILON); - - glm::vec3 expectedContact = sphereCenter - sphereRadius * edgeNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContact, EPSILON); - } - } - } - } - } -} - -void ShapeColliderTests::sphereTouchesAACubeCorners() { - CollisionList collisions(20); - - float sphereRadius = 1.37f; - glm::vec3 sphereCenter(0.0f); - - glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f); - float cubeSide = 2.98f; - - float overlap = 0.25f * sphereRadius; - int numSteps = 5; - - glm::vec3 faceNormals[] = {xAxis, yAxis, zAxis}; - - for (float firstSign = -1.0f; firstSign < 2.0f; firstSign += 2.0f) { - glm::vec3 firstNormal = firstSign * faceNormals[0]; - for (float secondSign = -1.0f; secondSign < 2.0f; secondSign += 2.0f) { - glm::vec3 secondNormal = secondSign * faceNormals[1]; - for (float thirdSign = -1.0f; thirdSign < 2.0f; thirdSign += 2.0f) { - collisions.clear(); - glm::vec3 thirdNormal = thirdSign * faceNormals[2]; - - // the cornerNormal is the normalized sum of the three faces - glm::vec3 cornerNormal = glm::normalize(firstNormal + secondNormal + thirdNormal); - - // compute a direction that is slightly offset from cornerNormal - glm::vec3 perpAxis = glm::normalize(glm::cross(cornerNormal, firstNormal)); - glm::vec3 nearbyAxis = glm::normalize(cornerNormal + 0.1f * perpAxis); - - // swing the sphere on a small cone that starts at the corner and is centered on the cornerNormal - float delta = TWO_PI / (float)(numSteps - 1); - for (int i = 0; i < numSteps; i++) { - float angle = (float)i * delta; - glm::quat rotation = glm::angleAxis(angle, cornerNormal); - glm::vec3 offsetAxis = rotation * nearbyAxis; - sphereCenter = cubeCenter + (SQUARE_ROOT_OF_3 * 0.5f * cubeSide) * cornerNormal + (sphereRadius - overlap) * offsetAxis; - - CollisionInfo* collision = ShapeCollider::sphereVsAACubeHelper(sphereCenter, sphereRadius, - cubeCenter, cubeSide, collisions); - - if (!collision) { - QFAIL_WITH_MESSAGE("sphere should collide with cube corner.\n\t\t" - << "cornerNormal = " << cornerNormal); - break; - } - - glm::vec3 expectedPenetration = - overlap * offsetAxis; - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, EPSILON); - - glm::vec3 expectedContact = sphereCenter - sphereRadius * offsetAxis; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContact, EPSILON); - } - } - } - } -} - -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; - - // capsule caps miss cube faces - for (int i = 0; i < numDirections; ++i) { - for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) { - glm::vec3 faceNormal = sign * faceNormals[i]; - glm::vec3 secondNormal = faceNormals[(i + 1) % numDirections]; - glm::vec3 thirdNormal = faceNormals[(i + 2) % numDirections]; - - // pick a random point somewhere above the face - glm::vec3 startPoint = cubeCenter + (cubeSide + capsuleRadius) * faceNormal + - (cubeSide * (randFloat() - 0.5f)) * secondNormal + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // pick a second random point slightly more than one radius above the face - glm::vec3 endPoint = cubeCenter + (0.5f * cubeSide + capsuleRadius + offset) * faceNormal + - (cubeSide * (randFloat() - 0.5f)) * secondNormal + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // randomly swap the points so capsule axis may point toward or away from face - if (randFloat() > 0.5f) { - glm::vec3 temp = startPoint; - startPoint = endPoint; - endPoint = temp; - } - - // create a capsule between the points - CapsuleShape capsule(capsuleRadius, startPoint, endPoint); - - // collide capsule with cube - QCOMPARE(ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions), false); - } - } - - // capsule caps miss cube edges - // loop over each face... - for (int i = 0; i < numDirections; ++i) { - for (float faceSign = -1.0f; faceSign < 2.0f; faceSign += 2.0f) { - glm::vec3 faceNormal = faceSign * faceNormals[i]; - - // loop over each neighboring face... - for (int j = (i + 1) % numDirections; j != i; j = (j + 1) % numDirections) { - // Compute the index to the third direction, which points perpendicular to both the face - // and the neighbor face. - int k = (j + 1) % numDirections; - if (k == i) { - k = (i + 1) % numDirections; - } - glm::vec3 thirdNormal = faceNormals[k]; - - collisions.clear(); - for (float neighborSign = -1.0f; neighborSign < 2.0f; neighborSign += 2.0f) { - glm::vec3 neighborNormal = neighborSign * faceNormals[j]; - - // combine the face and neighbor normals to get the edge normal - glm::vec3 edgeNormal = glm::normalize(faceNormal + neighborNormal); - - // pick a random point somewhere above the edge - glm::vec3 startPoint = cubeCenter + (SQUARE_ROOT_OF_2 * cubeSide + capsuleRadius) * edgeNormal + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // pick a second random point slightly more than one radius above the edge - glm::vec3 endPoint = cubeCenter + (SQUARE_ROOT_OF_2 * 0.5f * cubeSide + capsuleRadius + offset) * edgeNormal + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // randomly swap the points so capsule axis may point toward or away from edge - if (randFloat() > 0.5f) { - glm::vec3 temp = startPoint; - startPoint = endPoint; - endPoint = temp; - } - - // create a capsule between the points - CapsuleShape capsule(capsuleRadius, startPoint, endPoint); - - // collide capsule with cube - QCOMPARE(ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions), false); - } - } - } - } - - // capsule caps miss cube corners - for (float firstSign = -1.0f; firstSign < 2.0f; firstSign += 2.0f) { - glm::vec3 firstNormal = firstSign * faceNormals[0]; - for (float secondSign = -1.0f; secondSign < 2.0f; secondSign += 2.0f) { - glm::vec3 secondNormal = secondSign * faceNormals[1]; - for (float thirdSign = -1.0f; thirdSign < 2.0f; thirdSign += 2.0f) { - collisions.clear(); - glm::vec3 thirdNormal = thirdSign * faceNormals[2]; - - // the cornerNormal is the normalized sum of the three faces - glm::vec3 cornerNormal = glm::normalize(firstNormal + secondNormal + thirdNormal); - - // pick a random point somewhere above the corner - glm::vec3 startPoint = cubeCenter + (SQUARE_ROOT_OF_3 * cubeSide + capsuleRadius) * cornerNormal + - (0.25f * cubeSide * (randFloat() - 0.5f)) * firstNormal + - (0.25f * cubeSide * (randFloat() - 0.5f)) * secondNormal + - (0.25f * cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // 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 - QCOMPARE(ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions), false); - } - } - } - - // 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 - QCOMPARE(ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions), false); - } - } - } - } - - // 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 - QCOMPARE(ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions), false); - } - } - } - - // 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 - QCOMPARE(ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions), false); - } - } - } -} - -void ShapeColliderTests::capsuleTouchesAACube() { - CollisionList collisions(16); - - float capsuleRadius = 1.0f; - - glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f); - float cubeSide = 2.0f; - AACubeShape cube(cubeSide, cubeCenter); - - glm::vec3 faceNormals[] = {xAxis, yAxis, zAxis}; - int numDirections = 3; - - float overlap = 0.25f * capsuleRadius; - float allowableError = 10.0f * EPSILON; - - // capsule caps hit cube faces - for (int i = 0; i < numDirections; ++i) { - for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) { - glm::vec3 faceNormal = sign * faceNormals[i]; - glm::vec3 secondNormal = faceNormals[(i + 1) % numDirections]; - glm::vec3 thirdNormal = faceNormals[(i + 2) % numDirections]; - - // pick a random point somewhere above the face - glm::vec3 startPoint = cubeCenter + (cubeSide + capsuleRadius) * faceNormal + - (cubeSide * (randFloat() - 0.5f)) * secondNormal + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // pick a second random point slightly less than one radius above the face - // (but reduce width of range by 2*overlap to prevent the penetration from - // registering against other faces) - glm::vec3 endPoint = cubeCenter + (0.5f * cubeSide + capsuleRadius - overlap) * faceNormal + - ((cubeSide - 2.0f * overlap) * (randFloat() - 0.5f)) * secondNormal + - ((cubeSide - 2.0f * overlap) * (randFloat() - 0.5f)) * thirdNormal; - glm::vec3 collidingPoint = endPoint; - - // randomly swap the points so capsule axis may point toward or away from face - if (randFloat() > 0.5f) { - glm::vec3 temp = startPoint; - startPoint = endPoint; - endPoint = temp; - } - - // create a capsule between the points - CapsuleShape capsule(capsuleRadius, startPoint, endPoint); - - // collide capsule with cube - QCOMPARE(ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions), true); - - CollisionInfo* collision = collisions.getLastCollision(); - - // penetration points from capsule into cube - glm::vec3 expectedPenetration = - overlap * faceNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, allowableError); - - // contactPoint is on surface of capsule - glm::vec3 expectedContactPoint = collidingPoint - capsuleRadius * faceNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, allowableError); - } - } - - // capsule caps hit cube edges - // loop over each face... - for (int i = 0; i < numDirections; ++i) { - for (float faceSign = -1.0f; faceSign < 2.0f; faceSign += 2.0f) { - glm::vec3 faceNormal = faceSign * faceNormals[i]; - - // loop over each neighboring face... - for (int j = (i + 1) % numDirections; j != i; j = (j + 1) % numDirections) { - // Compute the index to the third direction, which points perpendicular to both the face - // and the neighbor face. - int k = (j + 1) % numDirections; - if (k == i) { - k = (i + 1) % numDirections; - } - glm::vec3 thirdNormal = faceNormals[k]; - - collisions.clear(); - for (float neighborSign = -1.0f; neighborSign < 2.0f; neighborSign += 2.0f) { - glm::vec3 neighborNormal = neighborSign * faceNormals[j]; - - // combine the face and neighbor normals to get the edge normal - glm::vec3 edgeNormal = glm::normalize(faceNormal + neighborNormal); - - // pick a random point somewhere above the edge - glm::vec3 startPoint = cubeCenter + (SQUARE_ROOT_OF_2 * cubeSide + capsuleRadius) * edgeNormal + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // pick a second random point slightly less than one radius above the edge - glm::vec3 endPoint = cubeCenter + (SQUARE_ROOT_OF_2 * 0.5f * cubeSide + capsuleRadius - overlap) * edgeNormal + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - glm::vec3 collidingPoint = endPoint; - - // randomly swap the points so capsule axis may point toward or away from edge - if (randFloat() > 0.5f) { - glm::vec3 temp = startPoint; - startPoint = endPoint; - endPoint = temp; - } - - // create a capsule between the points - CapsuleShape capsule(capsuleRadius, startPoint, endPoint); - - // collide capsule with cube - QCOMPARE(ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions), true); - - CollisionInfo* collision = collisions.getLastCollision(); - QCOMPARE(collision != nullptr, true); - - // penetration points from capsule into cube - glm::vec3 expectedPenetration = - overlap * edgeNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, allowableError); - - // contactPoint is on surface of capsule - glm::vec3 expectedContactPoint = collidingPoint - capsuleRadius * edgeNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, allowableError); - } - } - } - } - - // capsule caps hit cube corners - for (float firstSign = -1.0f; firstSign < 2.0f; firstSign += 2.0f) { - glm::vec3 firstNormal = firstSign * faceNormals[0]; - for (float secondSign = -1.0f; secondSign < 2.0f; secondSign += 2.0f) { - glm::vec3 secondNormal = secondSign * faceNormals[1]; - for (float thirdSign = -1.0f; thirdSign < 2.0f; thirdSign += 2.0f) { - collisions.clear(); - glm::vec3 thirdNormal = thirdSign * faceNormals[2]; - - // the cornerNormal is the normalized sum of the three faces - glm::vec3 cornerNormal = glm::normalize(firstNormal + secondNormal + thirdNormal); - - // pick a random point somewhere above the corner - glm::vec3 startPoint = cubeCenter + (SQUARE_ROOT_OF_3 * cubeSide + capsuleRadius) * cornerNormal + - (0.25f * cubeSide * (randFloat() - 0.5f)) * firstNormal + - (0.25f * cubeSide * (randFloat() - 0.5f)) * secondNormal + - (0.25f * cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // pick a second random point slightly less than one radius above the corner - glm::vec3 endPoint = cubeCenter + (SQUARE_ROOT_OF_3 * 0.5f * cubeSide + capsuleRadius - overlap) * cornerNormal; - glm::vec3 collidingPoint = endPoint; - - // randomly swap the points so capsule axis may point toward or away from corner - if (randFloat() > 0.5f) { - glm::vec3 temp = startPoint; - startPoint = endPoint; - endPoint = temp; - } - - // create a capsule between the points - CapsuleShape capsule(capsuleRadius, startPoint, endPoint); - - // collide capsule with cube - QCOMPARE(ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions), true); - - CollisionInfo* collision = collisions.getLastCollision(); - QCOMPARE(collision != nullptr, true); - - // penetration points from capsule into cube - glm::vec3 expectedPenetration = - overlap * cornerNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, allowableError); - - // contactPoint is on surface of capsule - glm::vec3 expectedContactPoint = collidingPoint - capsuleRadius * cornerNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, allowableError); - } - } - } - - // capsule sides hit cube edges - // loop over each face... - float capsuleLength = 2.0f; - for (int i = 0; i < numDirections; ++i) { - for (float faceSign = -1.0f; faceSign < 2.0f; faceSign += 2.0f) { - glm::vec3 faceNormal = faceSign * faceNormals[i]; - - // loop over each neighboring face... - for (int j = (i + 1) % numDirections; j != i; j = (j + 1) % numDirections) { - // Compute the index to the third direction, which points perpendicular to both the face - // and the neighbor face. - int k = (j + 1) % numDirections; - if (k == i) { - k = (i + 1) % numDirections; - } - glm::vec3 thirdNormal = faceNormals[k]; - - collisions.clear(); - for (float neighborSign = -1.0f; neighborSign < 2.0f; neighborSign += 2.0f) { - glm::vec3 neighborNormal = neighborSign * faceNormals[j]; - - // combine the face and neighbor normals to get the edge normal - glm::vec3 edgeNormal = glm::normalize(faceNormal + neighborNormal); - - // pick a random point somewhere along the edge - glm::vec3 edgePoint = cubeCenter + (SQUARE_ROOT_OF_2 * 0.5f * cubeSide) * edgeNormal + - ((cubeSide - 2.0f * overlap) * (randFloat() - 0.5f)) * thirdNormal; - - // pick a random normal that is deflected slightly from edgeNormal - glm::vec3 deflectedNormal = glm::normalize(edgeNormal + - (0.1f * (randFloat() - 0.5f)) * faceNormal + - (0.1f * (randFloat() - 0.5f)) * neighborNormal); - - // compute the axis direction, which will be perp to deflectedNormal and thirdNormal - glm::vec3 axisDirection = glm::normalize(glm::cross(deflectedNormal, thirdNormal)); - - // compute a point for the capsule's axis along deflection normal away from edgePoint - glm::vec3 axisPoint = edgePoint + (capsuleRadius - overlap) * deflectedNormal; - - // now we can compute the capsule endpoints - glm::vec3 endPoint = axisPoint + (0.5f * capsuleLength * randFloat()) * axisDirection; - glm::vec3 startPoint = axisPoint - (0.5f * capsuleLength * randFloat()) * axisDirection; - - // create a capsule between the points - CapsuleShape capsule(capsuleRadius, startPoint, endPoint); - - // collide capsule with cube - QCOMPARE(ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions), true); - - CollisionInfo* collision = collisions.getLastCollision(); - QCOMPARE(collision != nullptr, true); - - // penetration points from capsule into cube - glm::vec3 expectedPenetration = - overlap * deflectedNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, allowableError); - - // contactPoint is on surface of capsule - glm::vec3 expectedContactPoint = axisPoint - capsuleRadius * deflectedNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, allowableError); } - } - } - } - - // capsule sides hit cube corners - for (float firstSign = -1.0f; firstSign < 2.0f; firstSign += 2.0f) { - glm::vec3 firstNormal = firstSign * faceNormals[0]; - for (float secondSign = -1.0f; secondSign < 2.0f; secondSign += 2.0f) { - glm::vec3 secondNormal = secondSign * faceNormals[1]; - for (float thirdSign = -1.0f; thirdSign < 2.0f; thirdSign += 2.0f) { - collisions.clear(); - glm::vec3 thirdNormal = thirdSign * faceNormals[2]; - - // the cornerNormal is the normalized sum of the three faces - glm::vec3 cornerNormal = glm::normalize(firstNormal + secondNormal + thirdNormal); - - // compute a penetration normal that is somewhat randomized about cornerNormal - glm::vec3 penetrationNormal = - glm::normalize(cornerNormal + - (0.05f * cubeSide * (randFloat() - 0.5f)) * firstNormal + - (0.05f * cubeSide * (randFloat() - 0.5f)) * secondNormal + - (0.05f * cubeSide * (randFloat() - 0.5f)) * thirdNormal); - - // pick a random point somewhere above the corner - glm::vec3 corner = cubeCenter + (0.5f * cubeSide) * (firstNormal + secondNormal + thirdNormal); - glm::vec3 startPoint = corner + (3.0f * cubeSide) * cornerNormal + - (0.25f * cubeSide * (randFloat() - 0.5f)) * firstNormal + - (0.25f * cubeSide * (randFloat() - 0.5f)) * secondNormal + - (0.25f * cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // pick a second random point slightly less than one radius above the corner - // with some sight perp motion - glm::vec3 endPoint = corner - (capsuleRadius - overlap) * penetrationNormal; - glm::vec3 collidingPoint = endPoint; - - // randomly swap the points so capsule axis may point toward or away from corner - if (randFloat() > 0.5f) { - glm::vec3 temp = startPoint; - startPoint = endPoint; - endPoint = temp; - } - - // create a capsule between the points - CapsuleShape capsule(capsuleRadius, startPoint, endPoint); - - // collide capsule with cube - QCOMPARE(ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions), true); - - CollisionInfo* collision = collisions.getLastCollision(); - QCOMPARE(collision != nullptr, true); - - // penetration points from capsule into cube - glm::vec3 expectedPenetration = overlap * penetrationNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, allowableError); - - // contactPoint is on surface of capsule - glm::vec3 expectedContactPoint = collidingPoint + capsuleRadius * penetrationNormal; - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, allowableError); - } - } - } - - // 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 - QCOMPARE(ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions), true); - QCOMPARE(collisions.size(), 2); - - // 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; - QCOMPARE_WITH_ABS_ERROR(collision->_penetration, expectedPenetration, allowableError); - - // 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 - QCOMPARE_WITH_ABS_ERROR(collision->_contactPoint, expectedContactPoint, allowableError); - } - } - } - } -} - - -void ShapeColliderTests::rayHitsSphere() { - float startDistance = 3.0f; - - float radius = 1.0f; - glm::vec3 center(0.0f); - SphereShape sphere(radius, center); - - // very simple ray along xAxis - { - RayIntersectionInfo intersection; - intersection._rayStart = -startDistance * xAxis; - intersection._rayDirection = xAxis; - - QCOMPARE(sphere.findRayIntersection(intersection), true); - - float expectedDistance = startDistance - radius; - QCOMPARE_WITH_ABS_ERROR(intersection._hitDistance, expectedDistance, startDistance * EPSILON); - QCOMPARE(intersection._hitShape, &sphere); - } - - // ray along a diagonal axis - { - RayIntersectionInfo intersection; - intersection._rayStart = glm::vec3(startDistance, startDistance, 0.0f); - intersection._rayDirection = - glm::normalize(intersection._rayStart); - QCOMPARE(sphere.findRayIntersection(intersection), true); - - float expectedDistance = SQUARE_ROOT_OF_2 * startDistance - radius; - QCOMPARE_WITH_ABS_ERROR(intersection._hitDistance, expectedDistance, startDistance * EPSILON); - } - - // rotated and displaced ray and sphere - { - startDistance = 7.41f; - radius = 3.917f; - - glm::vec3 axis = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f)); - glm::quat rotation = glm::angleAxis(0.987654321f, axis); - glm::vec3 translation(35.7f, 2.46f, -1.97f); - - glm::vec3 unrotatedRayDirection = -xAxis; - glm::vec3 untransformedRayStart = startDistance * xAxis; - - RayIntersectionInfo intersection; - intersection._rayStart = rotation * (untransformedRayStart + translation); - intersection._rayDirection = rotation * unrotatedRayDirection; - - sphere.setRadius(radius); - sphere.setTranslation(rotation * translation); - - QCOMPARE(sphere.findRayIntersection(intersection), true); - float expectedDistance = startDistance - radius; - QCOMPARE_WITH_ABS_ERROR(intersection._hitDistance, expectedDistance, startDistance * EPSILON); - } -} - -void ShapeColliderTests::rayBarelyHitsSphere() { - float radius = 1.0f; - glm::vec3 center(0.0f); - float delta = 2.0f * EPSILON; - - SphereShape sphere(radius, center); - float startDistance = 3.0f; - - { - RayIntersectionInfo intersection; - intersection._rayStart = glm::vec3(-startDistance, radius - delta, 0.0f); - intersection._rayDirection = xAxis; - - // very simple ray along xAxis - QCOMPARE(sphere.findRayIntersection(intersection), true); - QCOMPARE(intersection._hitShape, &sphere); - } - - { - // translate and rotate the whole system... - glm::vec3 axis = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f)); - glm::quat rotation = glm::angleAxis(0.987654321f, axis); - glm::vec3 translation(35.7f, 0.46f, -1.97f); - - RayIntersectionInfo intersection; - intersection._rayStart = rotation * (intersection._rayStart + translation); - intersection._rayDirection = rotation * intersection._rayDirection; - - sphere.setTranslation(rotation * translation); - - // ...and test again - QCOMPARE(sphere.findRayIntersection(intersection), true); - } -} - - -void ShapeColliderTests::rayBarelyMissesSphere() { - // same as the barely-hits case, but this time we move the ray away from sphere - float radius = 1.0f; - glm::vec3 center(0.0f); - float delta = 2.0f * EPSILON; - - SphereShape sphere(radius, center); - float startDistance = 3.0f; - - { - RayIntersectionInfo intersection; - intersection._rayStart = glm::vec3(-startDistance, radius + delta, 0.0f); - intersection._rayDirection = xAxis; - - // very simple ray along xAxis - QCOMPARE(sphere.findRayIntersection(intersection), false); - QCOMPARE(intersection._hitDistance, FLT_MAX); - } - - { - // translate and rotate the whole system... - float angle = 0.987654321f; - glm::vec3 axis = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f)); - glm::quat rotation = glm::angleAxis(angle, axis); - glm::vec3 translation(35.7f, 2.46f, -1.97f); - - RayIntersectionInfo intersection; - intersection._rayStart = rotation * (glm::vec3(-startDistance, radius + delta, 0.0f) + translation); - intersection._rayDirection = rotation * xAxis; - sphere.setTranslation(rotation * translation); - - // ...and test again - QCOMPARE(sphere.findRayIntersection(intersection), false); - QCOMPARE(intersection._hitDistance == FLT_MAX, true); - QCOMPARE(intersection._hitShape == nullptr, true); - } -} - -void ShapeColliderTests::rayHitsCapsule() { - float startDistance = 3.0f; - float radius = 1.0f; - float halfHeight = 2.0f; - glm::vec3 center(0.0f); - CapsuleShape capsule(radius, halfHeight); - - // simple tests along xAxis - { // toward capsule center - RayIntersectionInfo intersection; - intersection._rayStart = glm::vec3(startDistance, 0.0f, 0.0f); - intersection._rayDirection = - xAxis; - QCOMPARE(capsule.findRayIntersection(intersection), true); - float expectedDistance = startDistance - radius; - QCOMPARE_WITH_ABS_ERROR(intersection._hitDistance, expectedDistance, startDistance * EPSILON); - QCOMPARE(intersection._hitShape, &capsule); - } - - { // toward top of cylindrical wall - RayIntersectionInfo intersection; - intersection._rayStart = glm::vec3(startDistance, halfHeight, 0.0f); - intersection._rayDirection = - xAxis; - QCOMPARE(capsule.findRayIntersection(intersection), true); - float expectedDistance = startDistance - radius; - QCOMPARE_WITH_ABS_ERROR(intersection._hitDistance, expectedDistance, startDistance * EPSILON); - } - - float delta = 2.0f * EPSILON; - { // toward top cap - RayIntersectionInfo intersection; - intersection._rayStart = glm::vec3(startDistance, halfHeight + delta, 0.0f); - intersection._rayDirection = - xAxis; - QCOMPARE(capsule.findRayIntersection(intersection), true); - float expectedDistance = startDistance - radius; - QCOMPARE_WITH_ABS_ERROR(intersection._hitDistance, expectedDistance, startDistance * EPSILON); - } - - const float EDGE_CASE_SLOP_FACTOR = 20.0f; - { // toward tip of top cap - RayIntersectionInfo intersection; - intersection._rayStart = glm::vec3(startDistance, halfHeight + radius - delta, 0.0f); - intersection._rayDirection = - xAxis; - QCOMPARE(capsule.findRayIntersection(intersection), true); - float expectedDistance = startDistance - radius * sqrtf(2.0f * delta); // using small angle approximation of cosine -// float relativeError = fabsf(intersection._hitDistance - expectedDistance) / startDistance; - // for edge cases we allow a LOT of error - QCOMPARE_WITH_ABS_ERROR(intersection._hitDistance, expectedDistance, startDistance * EDGE_CASE_SLOP_FACTOR * EPSILON); - } - - { // toward tip of bottom cap - RayIntersectionInfo intersection; - intersection._rayStart = glm::vec3(startDistance, - halfHeight - radius + delta, 0.0f); - intersection._rayDirection = - xAxis; - QCOMPARE(capsule.findRayIntersection(intersection), true); - float expectedDistance = startDistance - radius * sqrtf(2.0f * delta); // using small angle approximation of cosine -// float relativeError = fabsf(intersection._hitDistance - expectedDistance) / startDistance; - // for edge cases we allow a LOT of error - QCOMPARE_WITH_ABS_ERROR(intersection._hitDistance, expectedDistance, startDistance * EPSILON * EDGE_CASE_SLOP_FACTOR); - } - - { // toward edge of capsule cylindrical face - RayIntersectionInfo intersection; - intersection._rayStart = glm::vec3(startDistance, 0.0f, radius - delta); - intersection._rayDirection = - xAxis; - QCOMPARE(capsule.findRayIntersection(intersection), true); - float expectedDistance = startDistance - radius * sqrtf(2.0f * delta); // using small angle approximation of cosine - float relativeError = fabsf(intersection._hitDistance - expectedDistance) / startDistance; - // for edge cases we allow a LOT of error - QCOMPARE_WITH_ABS_ERROR(intersection._hitDistance, expectedDistance, startDistance * EPSILON * EDGE_CASE_SLOP_FACTOR); - } - // TODO: test at steep angles near cylinder/cap junction -} - -void ShapeColliderTests::rayMissesCapsule() { - // same as edge case hit tests, but shifted in the opposite direction - float startDistance = 3.0f; - float radius = 1.0f; - float halfHeight = 2.0f; - glm::vec3 center(0.0f); - CapsuleShape capsule(radius, halfHeight); - - { // simple test along xAxis - // toward capsule center - RayIntersectionInfo intersection; - intersection._rayStart = glm::vec3(startDistance, 0.0f, 0.0f); - intersection._rayDirection = -xAxis; - float delta = 2.0f * EPSILON; - - // over top cap - intersection._rayStart.y = halfHeight + radius + delta; - intersection._hitDistance = FLT_MAX; - QCOMPARE(capsule.findRayIntersection(intersection), false); - QCOMPARE(intersection._hitDistance, FLT_MAX); - - // below bottom cap - intersection._rayStart.y = - halfHeight - radius - delta; - intersection._hitDistance = FLT_MAX; - QCOMPARE(capsule.findRayIntersection(intersection), false); - QCOMPARE(intersection._hitDistance, FLT_MAX); - - // past edge of capsule cylindrical face - intersection._rayStart.y = 0.0f; - intersection._rayStart.z = radius + delta; - intersection._hitDistance = FLT_MAX; - QCOMPARE(capsule.findRayIntersection(intersection), false); - QCOMPARE(intersection._hitDistance, FLT_MAX); - QCOMPARE(intersection._hitShape, (Shape*)nullptr); - } - // TODO: test at steep angles near edge -} - -void ShapeColliderTests::rayHitsPlane() { - // make a simple plane - float planeDistanceFromOrigin = 3.579f; - glm::vec3 planePosition(0.0f, planeDistanceFromOrigin, 0.0f); - PlaneShape plane; - plane.setPoint(planePosition); - plane.setNormal(yAxis); - - // make a simple ray - float startDistance = 1.234f; - { - RayIntersectionInfo intersection; - intersection._rayStart = -startDistance * xAxis; - intersection._rayDirection = glm::normalize(glm::vec3(1.0f, 1.0f, 1.0f)); - - QCOMPARE(plane.findRayIntersection(intersection), true); - float expectedDistance = SQUARE_ROOT_OF_3 * planeDistanceFromOrigin; - - QCOMPARE_WITH_ABS_ERROR(intersection._hitDistance, expectedDistance, planeDistanceFromOrigin * EPSILON); - QCOMPARE(intersection._hitShape, &plane); - } - - { // rotate the whole system and try again - float angle = 37.8f; - glm::vec3 axis = glm::normalize( glm::vec3(-7.0f, 2.8f, 9.3f) ); - glm::quat rotation = glm::angleAxis(angle, axis); - - plane.setNormal(rotation * yAxis); - plane.setPoint(rotation * planePosition); - RayIntersectionInfo intersection; - intersection._rayStart = rotation * (-startDistance * xAxis); - intersection._rayDirection = rotation * glm::normalize(glm::vec3(1.0f, 1.0f, 1.0f)); - - QCOMPARE(plane.findRayIntersection(intersection), true); - - float expectedDistance = SQUARE_ROOT_OF_3 * planeDistanceFromOrigin; - QCOMPARE_WITH_ABS_ERROR(intersection._hitDistance, expectedDistance, planeDistanceFromOrigin * EPSILON); - } -} - -void ShapeColliderTests::rayMissesPlane() { - // make a simple plane - float planeDistanceFromOrigin = 3.579f; - glm::vec3 planePosition(0.0f, planeDistanceFromOrigin, 0.0f); - PlaneShape plane; - plane.setTranslation(planePosition); - - { // parallel rays should miss - float startDistance = 1.234f; - RayIntersectionInfo intersection; - intersection._rayStart = glm::vec3(-startDistance, 0.0f, 0.0f); - intersection._rayDirection = glm::normalize(glm::vec3(-1.0f, 0.0f, -1.0f)); - - QCOMPARE(plane.findRayIntersection(intersection), false); - QCOMPARE(intersection._hitDistance, FLT_MAX); - - // rotate the whole system and try again - float angle = 37.8f; - glm::vec3 axis = glm::normalize( glm::vec3(-7.0f, 2.8f, 9.3f) ); - glm::quat rotation = glm::angleAxis(angle, axis); - - plane.setTranslation(rotation * planePosition); - plane.setRotation(rotation); - - intersection._rayStart = rotation * intersection._rayStart; - intersection._rayDirection = rotation * intersection._rayDirection; - intersection._hitDistance = FLT_MAX; - - QCOMPARE(plane.findRayIntersection(intersection), false); - QCOMPARE(intersection._hitDistance, FLT_MAX); - QCOMPARE(intersection._hitShape, (Shape*)nullptr); - } - - { // make a simple ray that points away from plane - float startDistance = 1.234f; - - RayIntersectionInfo intersection; - intersection._rayStart = glm::vec3(-startDistance, 0.0f, 0.0f); - intersection._rayDirection = glm::normalize(glm::vec3(-1.0f, -1.0f, -1.0f)); - intersection._hitDistance = FLT_MAX; - - QCOMPARE(plane.findRayIntersection(intersection), false); - QCOMPARE(intersection._hitDistance, FLT_MAX); - - // rotate the whole system and try again - float angle = 37.8f; - glm::vec3 axis = glm::normalize( glm::vec3(-7.0f, 2.8f, 9.3f) ); - glm::quat rotation = glm::angleAxis(angle, axis); - - plane.setTranslation(rotation * planePosition); - plane.setRotation(rotation); - - intersection._rayStart = rotation * intersection._rayStart; - intersection._rayDirection = rotation * intersection._rayDirection; - intersection._hitDistance = FLT_MAX; - - QCOMPARE(plane.findRayIntersection(intersection), false); - QCOMPARE(intersection._hitDistance, FLT_MAX); - } -} - -void ShapeColliderTests::rayHitsAACube() { - glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f); - float cubeSide = 2.127f; - AACubeShape cube(cubeSide, cubeCenter); - - float rayOffset = 3.796f; - - glm::vec3 faceNormals[] = {xAxis, yAxis, zAxis}; - int numDirections = 3; - int numRayCasts = 5; - - 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 rayStart = cubeCenter + - (cubeSide + rayOffset) * faceNormal + - (cubeSide * (randFloat() - 0.5f)) * secondNormal + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // cast multiple rays toward the face - for (int j = 0; j < numRayCasts; ++j) { - // pick a random point on the face - glm::vec3 facePoint = cubeCenter + - 0.5f * cubeSide * faceNormal + - (cubeSide * (randFloat() - 0.5f)) * secondNormal + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // construct a ray from first point through second point - RayIntersectionInfo intersection; - intersection._rayStart = rayStart; - intersection._rayDirection = glm::normalize(facePoint - rayStart); - intersection._rayLength = 1.0001f * glm::distance(rayStart, facePoint); - - // cast the ray -// bool hit = cube.findRayIntersection(intersection); - - // validate - QCOMPARE(cube.findRayIntersection(intersection), true); - QCOMPARE_WITH_ABS_ERROR(glm::dot(faceNormal, intersection._hitNormal), 1.0f, EPSILON); - QCOMPARE_WITH_ABS_ERROR(facePoint, intersection.getIntersectionPoint(), EPSILON); - QCOMPARE(intersection._hitShape, &cube); - } - } - } -} - -void ShapeColliderTests::rayMissesAACube() { - //glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f); - //float cubeSide = 2.127f; - glm::vec3 cubeCenter(0.0f); - float cubeSide = 2.0f; - AACubeShape cube(cubeSide, cubeCenter); - - float rayOffset = 3.796f; - - glm::vec3 faceNormals[] = {xAxis, yAxis, zAxis}; - int numDirections = 3; - int numRayCasts = 5; - - const float SOME_SMALL_NUMBER = 0.0001f; - - { // ray misses cube for being too short - 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 rayStart = cubeCenter + - (cubeSide + rayOffset) * faceNormal + - (cubeSide * (randFloat() - 0.5f)) * secondNormal + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // cast multiple rays toward the face - for (int j = 0; j < numRayCasts; ++j) { - // pick a random point on the face - glm::vec3 facePoint = cubeCenter + - 0.5f * cubeSide * faceNormal + - (cubeSide * (randFloat() - 0.5f)) * secondNormal + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // construct a ray from first point to almost second point - RayIntersectionInfo intersection; - intersection._rayStart = rayStart; - intersection._rayDirection = glm::normalize(facePoint - rayStart); - intersection._rayLength = (1.0f - SOME_SMALL_NUMBER) * glm::distance(rayStart, facePoint); - - // cast the ray - QCOMPARE(cube.findRayIntersection(intersection), false); - } - } - } - } - { // long ray misses cube - 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 rayStart = cubeCenter + - (cubeSide + rayOffset) * faceNormal + - (cubeSide * (randFloat() - 0.5f)) * secondNormal + - (cubeSide * (randFloat() - 0.5f)) * thirdNormal; - - // cast multiple rays that miss the face - for (int j = 0; j < numRayCasts; ++j) { - // pick a random point just outside of face - float inside = (cubeSide * (randFloat() - 0.5f)); - float outside = 0.5f * cubeSide + SOME_SMALL_NUMBER * randFloat(); - if (randFloat() - 0.5f < 0.0f) { - outside *= -1.0f; - } - glm::vec3 sidePoint = cubeCenter + 0.5f * cubeSide * faceNormal; - if (randFloat() - 0.5f < 0.0f) { - sidePoint += outside * secondNormal + inside * thirdNormal; - } else { - sidePoint += inside * secondNormal + outside * thirdNormal; - } - - // construct a ray from first point through second point - RayIntersectionInfo intersection; - intersection._rayStart = rayStart; - intersection._rayDirection = glm::normalize(sidePoint - rayStart); - - // cast the ray - QCOMPARE(cube.findRayIntersection(intersection), false); - } - } - } - } - { // ray parallel to face barely misses cube - 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]; - - // cast multiple rays that miss the face - for (int j = 0; j < numRayCasts; ++j) { - // rayStart is above the face - glm::vec3 rayStart = cubeCenter + (0.5f + SOME_SMALL_NUMBER) * cubeSide * faceNormal; - - // move rayStart to some random edge and choose the ray direction to point across the face - float inside = (cubeSide * (randFloat() - 0.5f)); - float outside = 0.5f * cubeSide + SOME_SMALL_NUMBER * randFloat(); - if (randFloat() - 0.5f < 0.0f) { - outside *= -1.0f; - } - glm::vec3 rayDirection = secondNormal; - if (randFloat() - 0.5f < 0.0f) { - rayStart += outside * secondNormal + inside * thirdNormal; - } else { - rayStart += inside * secondNormal + outside * thirdNormal; - rayDirection = thirdNormal; - } - if (outside > 0.0f) { - rayDirection *= -1.0f; - } - - // construct a ray from first point through second point - RayIntersectionInfo intersection; - intersection._rayStart = rayStart; - intersection._rayDirection = rayDirection; - - // cast the ray - QCOMPARE(cube.findRayIntersection(intersection), false); - } - } - } - } - -} - -void ShapeColliderTests::measureTimeOfCollisionDispatch() { - - // TODO: use QBENCHMARK for this - - /* KEEP for future manual testing - // create two non-colliding spheres - float radiusA = 7.0f; - float radiusB = 3.0f; - float alpha = 1.2f; - float beta = 1.3f; - glm::vec3 offsetDirection = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f)); - float offsetDistance = alpha * radiusA + beta * radiusB; - - SphereShape sphereA(radiusA, origin); - SphereShape sphereB(radiusB, offsetDistance * offsetDirection); - CollisionList collisions(16); - - //int numTests = 1; - quint64 oldTime; - quint64 newTime; - int numTests = 100000000; - { - quint64 startTime = usecTimestampNow(); - for (int i = 0; i < numTests; ++i) { - ShapeCollider::collideShapes(&sphereA, &sphereB, collisions); - } - quint64 endTime = usecTimestampNow(); - std::cout << numTests << " non-colliding collisions in " << (endTime - startTime) << " usec" << std::endl; - newTime = endTime - startTime; - } - */ -} - diff --git a/tests/physics/src/ShapeColliderTests.h b/tests/physics/src/ShapeColliderTests.h deleted file mode 100644 index 48d9cbd742..0000000000 --- a/tests/physics/src/ShapeColliderTests.h +++ /dev/null @@ -1,60 +0,0 @@ -// -// ShapeColliderTests.h -// tests/physics/src -// -// Created by Andrew Meadows on 02/21/2014. -// 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_ShapeColliderTests_h -#define hifi_ShapeColliderTests_h - -#include -#include - -// Add additional qtest functionality (the include order is important!) -#include "BulletTestUtils.h" -#include "GlmTestUtils.h" -#include "../QTestExtensions.h" - - -class ShapeColliderTests : public QObject { - Q_OBJECT - -private slots: - void initTestCase(); - - void sphereMissesSphere(); - void sphereTouchesSphere(); - - void sphereMissesCapsule(); - void sphereTouchesCapsule(); - - void capsuleMissesCapsule(); - void capsuleTouchesCapsule(); - - void sphereTouchesAACubeFaces(); - void sphereTouchesAACubeEdges(); - void sphereTouchesAACubeCorners(); - void sphereMissesAACube(); - - void capsuleMissesAACube(); - void capsuleTouchesAACube(); - - void rayHitsSphere(); - void rayBarelyHitsSphere(); - void rayBarelyMissesSphere(); - void rayHitsCapsule(); - void rayMissesCapsule(); - void rayHitsPlane(); - void rayMissesPlane(); - void rayHitsAACube(); - void rayMissesAACube(); - - void measureTimeOfCollisionDispatch(); -}; - -#endif // hifi_ShapeColliderTests_h From 85f06639a5a67117be3c7ff644f4ffc771d97123 Mon Sep 17 00:00:00 2001 From: Niels Nesse Date: Thu, 30 Jul 2015 12:50:57 -0700 Subject: [PATCH 05/11] Remove redundant code in getOrCreateChildElementAt() --- libraries/octree/src/OctreeElement.cpp | 45 +------------------------- 1 file changed, 1 insertion(+), 44 deletions(-) diff --git a/libraries/octree/src/OctreeElement.cpp b/libraries/octree/src/OctreeElement.cpp index e11a11fc8e..d166b886b8 100644 --- a/libraries/octree/src/OctreeElement.cpp +++ b/libraries/octree/src/OctreeElement.cpp @@ -640,50 +640,7 @@ OctreeElement* OctreeElement::getOrCreateChildElementAt(float x, float y, float // otherwise, we need to find which of our children we should recurse glm::vec3 ourCenter = _cube.calcCenter(); - int childIndex = CHILD_UNKNOWN; - // left half - if (x > ourCenter.x) { - if (y > ourCenter.y) { - // top left - if (z > ourCenter.z) { - // top left far - childIndex = CHILD_TOP_LEFT_FAR; - } else { - // top left near - childIndex = CHILD_TOP_LEFT_NEAR; - } - } else { - // bottom left - if (z > ourCenter.z) { - // bottom left far - childIndex = CHILD_BOTTOM_LEFT_FAR; - } else { - // bottom left near - childIndex = CHILD_BOTTOM_LEFT_NEAR; - } - } - } else { - // right half - if (y > ourCenter.y) { - // top right - if (z > ourCenter.z) { - // top right far - childIndex = CHILD_TOP_RIGHT_FAR; - } else { - // top right near - childIndex = CHILD_TOP_RIGHT_NEAR; - } - } else { - // bottom right - if (z > ourCenter.z) { - // bottom right far - childIndex = CHILD_BOTTOM_RIGHT_FAR; - } else { - // bottom right near - childIndex = CHILD_BOTTOM_RIGHT_NEAR; - } - } - } + int childIndex = getMyChildContainingPoint(glm::vec3(x, y, z)); // Now, check if we have a child at that location child = getChildAtIndex(childIndex); From 4c1d1a65d50aab90ca19ccc5d482c1c28732f9e6 Mon Sep 17 00:00:00 2001 From: Howard Stearns Date: Mon, 3 Aug 2015 15:41:15 -0700 Subject: [PATCH 06/11] Provide initial fight club animations and use them in rig. Still only active when you do Settings.setValue('enableRig', true) and restart. (Will be more exposed after fadein/fadeout is implemented.) --- libraries/animation/src/Rig.cpp | 100 ++++++++++++++++++++++---------- libraries/animation/src/Rig.h | 4 +- 2 files changed, 70 insertions(+), 34 deletions(-) diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 8406b61d12..45828d44b6 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -90,8 +90,34 @@ void Rig::addAnimationByRole(const QString& role, const QString& url, float fps, } } AnimationHandlePointer handle = createAnimationHandle(); + QString standard = ""; + if (url.isEmpty()) { // Default animations for fight club + const QString& base = "https://hifi-public.s3.amazonaws.com/ozan/"; + if (role == "walk") { + standard = base + "support/FightClubBotTest1/Animations/standard_walk.fbx"; + lastFrame = 60; + } else if (role == "leftTurn") { + standard = base + "support/FightClubBotTest1/Animations/left_turn_noHipRotation.fbx"; + lastFrame = 29; + } else if (role == "rightTurn") { + standard = base + "support/FightClubBotTest1/Animations/right_turn_noHipRotation.fbx"; + lastFrame = 31; + } else if (role == "leftStrafe") { + standard = base + "animations/fightclub_bot_anims/side_step_left_inPlace.fbx"; + lastFrame = 31; + } else if (role == "rightStrafe") { + standard = base + "animations/fightclub_bot_anims/side_step_right_inPlace.fbx"; + lastFrame = 31; + } else if (role == "idle") { + standard = base + "support/FightClubBotTest1/Animations/standard_idle.fbx"; + fps = 25.0f; + } + if (!standard.isEmpty()) { + loop = true; + } + } handle->setRole(role); - handle->setURL(url); + handle->setURL(url.isEmpty() ? standard : url); handle->setFPS(fps); handle->setPriority(priority); handle->setLoop(loop); @@ -135,6 +161,14 @@ void Rig::addRunningAnimation(AnimationHandlePointer animationHandle) { bool Rig::isRunningAnimation(AnimationHandlePointer animationHandle) { return _runningAnimations.contains(animationHandle); } +bool Rig::isRunningRole(const QString& role) { //obviously, there are more efficient ways to do this + for (auto animation : _runningAnimations) { + if (animation->getRole() == role) { + return true; + } + } + return false; +} void Rig::deleteAnimations() { for (auto animation : _animationHandles) { @@ -356,37 +390,41 @@ glm::mat4 Rig::getJointVisibleTransform(int jointIndex) const { } void Rig::computeMotionAnimationState(float deltaTime, const glm::vec3& worldPosition, const glm::vec3& worldVelocity, const glm::quat& worldRotation) { - if (_enableRig) { - glm::vec3 front = worldRotation * IDENTITY_FRONT; - float forwardSpeed = glm::dot(worldVelocity, front); - float rotationalSpeed = glm::angle(front, _lastFront) / deltaTime; - bool isWalking = std::abs(forwardSpeed) > 0.01f; - bool isTurning = std::abs(rotationalSpeed) > 0.5f; - - // Crude, until we have blending: - isTurning = isTurning && !isWalking; // Only one of walk/turn, walk wins. - isTurning = false; // FIXME - bool isIdle = !isWalking && !isTurning; - auto singleRole = [](bool walking, bool turning, bool idling) { - return walking ? "walk" : (turning ? "turn" : (idling ? "idle" : "")); - }; - QString toStop = singleRole(_isWalking && !isWalking, _isTurning && !isTurning, _isIdle && !isIdle); - if (!toStop.isEmpty()) { - //qCDebug(animation) << "isTurning" << isTurning << "fronts" << front << _lastFront << glm::angle(front, _lastFront) << rotationalSpeed; - stopAnimationByRole(toStop); - } - QString newRole = singleRole(isWalking && !_isWalking, isTurning && !_isTurning, isIdle && !_isIdle); - if (!newRole.isEmpty()) { - startAnimationByRole(newRole); - qCDebug(animation) << deltaTime << ":" << worldVelocity << "." << front << "=> " << forwardSpeed << newRole; - } - - _lastPosition = worldPosition; - _lastFront = front; - _isWalking = isWalking; - _isTurning = isTurning; - _isIdle = isIdle; + if (!_enableRig) { + return; } + bool isMoving = false; + glm::vec3 front = worldRotation * IDENTITY_FRONT; + float forwardSpeed = glm::dot(worldVelocity, front); + float rightLateralSpeed = glm::dot(worldVelocity, worldRotation * IDENTITY_RIGHT); + float rightTurningSpeed = glm::orientedAngle(front, _lastFront, IDENTITY_UP) / deltaTime; + auto updateRole = [=](const QString& role, bool isOn) mutable { + isMoving = isMoving || isOn; + if (isOn) { + if (!isRunningRole(role)) { + qCDebug(animation) << "Rig STARTING" << role; + if (role == "leftTurn" || role == "rightTurn") { + qCDebug(animation) << front.x << front.y << front.z << "=>" << rightTurningSpeed; + } + startAnimationByRole(role); + } + } else { + if (isRunningRole(role)) { + qCDebug(animation) << "Rig stopping" << role; + stopAnimationByRole(role); + } + } + }; + updateRole("walk", std::abs(forwardSpeed) > 0.01f); + bool isTurning = std::abs(rightTurningSpeed) > 0.5f; + updateRole("rightTurn", isTurning && (rightTurningSpeed > 0)); + updateRole("leftTurn", isTurning && (rightTurningSpeed < 0)); + bool isStrafing = std::abs(rightLateralSpeed) > 0.01f; + updateRole("rightStrafe", isStrafing && (rightLateralSpeed > 0.0f)); + updateRole("leftStrafe", isStrafing && (rightLateralSpeed < 0.0f)); + updateRole("idle", !isMoving); // Must be last, as it makes isMoving bogus. + _lastFront = front; + _lastPosition = worldPosition; } void Rig::updateAnimations(float deltaTime, glm::mat4 parentTransform) { diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 52db16826a..a79b99f4dc 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -76,6 +76,7 @@ public: bool removeRunningAnimation(AnimationHandlePointer animationHandle); void addRunningAnimation(AnimationHandlePointer animationHandle); bool isRunningAnimation(AnimationHandlePointer animationHandle); + bool isRunningRole(const QString& role); // There can be multiple animations per role, so this is more general than isRunningAnimation. const QList& getRunningAnimations() const { return _runningAnimations; } void deleteAnimations(); const QList& getAnimationHandles() const { return _animationHandles; } @@ -161,9 +162,6 @@ public: QList _runningAnimations; bool _enableRig; - bool _isWalking; - bool _isTurning; - bool _isIdle; glm::vec3 _lastFront; glm::vec3 _lastPosition; }; From cdb697760f30f738c79ace5ab87cea573f8e334c Mon Sep 17 00:00:00 2001 From: Howard Stearns Date: Mon, 3 Aug 2015 15:51:07 -0700 Subject: [PATCH 07/11] Remove some debug. --- libraries/animation/src/Rig.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 45828d44b6..4a03ab83ee 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -403,9 +403,6 @@ void Rig::computeMotionAnimationState(float deltaTime, const glm::vec3& worldPos if (isOn) { if (!isRunningRole(role)) { qCDebug(animation) << "Rig STARTING" << role; - if (role == "leftTurn" || role == "rightTurn") { - qCDebug(animation) << front.x << front.y << front.z << "=>" << rightTurningSpeed; - } startAnimationByRole(role); } } else { From 2e5142e9bb5110544fea62f597039cf9021bc0ea Mon Sep 17 00:00:00 2001 From: Howard Stearns Date: Mon, 3 Aug 2015 16:09:48 -0700 Subject: [PATCH 08/11] Fix end of idle. --- libraries/animation/src/Rig.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 4a03ab83ee..f8419509d4 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -398,7 +398,7 @@ void Rig::computeMotionAnimationState(float deltaTime, const glm::vec3& worldPos float forwardSpeed = glm::dot(worldVelocity, front); float rightLateralSpeed = glm::dot(worldVelocity, worldRotation * IDENTITY_RIGHT); float rightTurningSpeed = glm::orientedAngle(front, _lastFront, IDENTITY_UP) / deltaTime; - auto updateRole = [=](const QString& role, bool isOn) mutable { + auto updateRole = [&](const QString& role, bool isOn) { isMoving = isMoving || isOn; if (isOn) { if (!isRunningRole(role)) { From 61afc362e770d3287832aa390696b4b9ab5bdf7e Mon Sep 17 00:00:00 2001 From: Howard Stearns Date: Mon, 3 Aug 2015 17:00:39 -0700 Subject: [PATCH 09/11] For startAnimation (but not startAnimationByRole), use the specified parameters even if the animation was already playing. This fixes the behavior exercised by the squeezeHands.js script. Fixes https://app.asana.com/0/32622044445063/44025709513292 --- libraries/animation/src/Rig.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 8406b61d12..b91606dc8e 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -58,15 +58,18 @@ void Rig::removeAnimationHandle(const AnimationHandlePointer& handle) { void Rig::startAnimation(const QString& url, float fps, float priority, bool loop, bool hold, float firstFrame, float lastFrame, const QStringList& maskedJoints) { - //qCDebug(animation) << "startAnimation" << url << fps << priority << loop << hold << firstFrame << lastFrame << maskedJoints; + // This is different than startAnimationByRole, in which we use the existing values if the animation already exists. + // Here we reuse the animation handle if possible, but in any case, we set the values to those given (or defaulted). + AnimationHandlePointer handle = nullptr; foreach (const AnimationHandlePointer& candidate, _animationHandles) { if (candidate->getURL() == url) { - candidate->start(); - return; + handle = candidate; } } - AnimationHandlePointer handle = createAnimationHandle(); - handle->setURL(url); + if (!handle) { + handle = createAnimationHandle(); + handle->setURL(url); + } handle->setFPS(fps); handle->setPriority(priority); handle->setLoop(loop); From 52be44d6215693c88b4737cdc9de2067709e246d Mon Sep 17 00:00:00 2001 From: SamGondelman Date: Tue, 4 Aug 2015 14:50:38 -0700 Subject: [PATCH 10/11] uninitialized value --- interface/src/scripting/ControllerScriptingInterface.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/interface/src/scripting/ControllerScriptingInterface.cpp b/interface/src/scripting/ControllerScriptingInterface.cpp index 7a3f4a99b5..feceecc3fd 100644 --- a/interface/src/scripting/ControllerScriptingInterface.cpp +++ b/interface/src/scripting/ControllerScriptingInterface.cpp @@ -24,7 +24,8 @@ ControllerScriptingInterface::ControllerScriptingInterface() : _mouseCaptured(false), _touchCaptured(false), - _wheelCaptured(false) + _wheelCaptured(false), + _actionsCaptured(false) { } From 241ee00ca57dc58bd4967b46ae276a9326696ccf Mon Sep 17 00:00:00 2001 From: Bradley Austin Davis Date: Tue, 4 Aug 2015 15:18:01 -0700 Subject: [PATCH 11/11] Working on HMD crash on mac --- libraries/shared/src/GLHelpers.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/libraries/shared/src/GLHelpers.cpp b/libraries/shared/src/GLHelpers.cpp index f2371e49bb..f67f9e9120 100644 --- a/libraries/shared/src/GLHelpers.cpp +++ b/libraries/shared/src/GLHelpers.cpp @@ -10,7 +10,6 @@ QSurfaceFormat getDefaultOpenGlSurfaceFormat() { #ifdef DEBUG format.setOption(QSurfaceFormat::DebugContext); #endif - // FIXME move to core as soon as possible - format.setProfile(QSurfaceFormat::OpenGLContextProfile::CompatibilityProfile); + format.setProfile(QSurfaceFormat::OpenGLContextProfile::CoreProfile); return format; }