diff --git a/libraries/shared/src/AABox.cpp b/libraries/octree/src/AABox.cpp similarity index 100% rename from libraries/shared/src/AABox.cpp rename to libraries/octree/src/AABox.cpp diff --git a/libraries/shared/src/AABox.h b/libraries/octree/src/AABox.h similarity index 100% rename from libraries/shared/src/AABox.h rename to libraries/octree/src/AABox.h diff --git a/libraries/octree/src/Octree.cpp b/libraries/octree/src/Octree.cpp index a06b8a41bb..83b84b3c50 100644 --- a/libraries/octree/src/Octree.cpp +++ b/libraries/octree/src/Octree.cpp @@ -710,16 +710,16 @@ bool findShapeCollisionsOp(OctreeElement* node, void* extraData) { const ShapeArgs* args = static_cast(extraData); // coarse check against bounds - AABox box = node->getAABox(); - box.scale(TREE_SCALE); - if (!box.expandedContains(args->shape->getPosition(), args->shape->getBoundingRadius())) { + AABox cube = node->getAABox(); + cube.scale(TREE_SCALE); + if (!cube.expandedContains(args->shape->getPosition(), args->shape->getBoundingRadius())) { return false; } if (!node->isLeaf()) { return true; // recurse on children } if (node->hasContent()) { - return ShapeCollider::collideShapeWithBox(args->shape, box, args->collisions); + return ShapeCollider::collideShapeWithAACube(args->shape, cube.calcCenter(), cube.getScale(), args->collisions); } return false; } diff --git a/libraries/octree/src/OctreeElement.cpp b/libraries/octree/src/OctreeElement.cpp index c951803247..7ca15eddb5 100644 --- a/libraries/octree/src/OctreeElement.cpp +++ b/libraries/octree/src/OctreeElement.cpp @@ -15,16 +15,16 @@ #include -#include #include #include #include +#include "AABox.h" #include "OctalCode.h" -#include "SharedUtil.h" #include "OctreeConstants.h" #include "OctreeElement.h" #include "Octree.h" +#include "SharedUtil.h" quint64 OctreeElement::_voxelMemoryUsage = 0; quint64 OctreeElement::_octcodeMemoryUsage = 0; diff --git a/libraries/octree/src/OctreeElement.h b/libraries/octree/src/OctreeElement.h index 15d5063818..c5eec1c9e2 100644 --- a/libraries/octree/src/OctreeElement.h +++ b/libraries/octree/src/OctreeElement.h @@ -18,9 +18,9 @@ #include -#include #include +#include "AABox.h" #include "ViewFrustum.h" #include "OctreeConstants.h" //#include "Octree.h" diff --git a/libraries/octree/src/ViewFrustum.h b/libraries/octree/src/ViewFrustum.h index 43812827f5..acd5c639f7 100644 --- a/libraries/octree/src/ViewFrustum.h +++ b/libraries/octree/src/ViewFrustum.h @@ -17,9 +17,8 @@ #include #include -#include +#include "AABox.h" #include "Plane.h" - #include "OctreeConstants.h" #include "OctreeProjectedPolygon.h" diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index 0eddc80c62..1b49caab22 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -92,8 +92,26 @@ bool collideShapesCoarse(const QVector& shapesA, const QVectorgetType(); + if (typeA == Shape::SPHERE_SHAPE) { + return sphereAACube(static_cast(shapeA), cubeCenter, cubeSide, collisions); + } else if (typeA == Shape::CAPSULE_SHAPE) { + return capsuleAACube(static_cast(shapeA), cubeCenter, cubeSide, collisions); + } else if (typeA == Shape::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 == Shape::SPHERE_SHAPE) { + touching = sphereAACube(static_cast(subShape), cubeCenter, cubeSide, collisions) || touching; + } else if (subType == Shape::CAPSULE_SHAPE) { + touching = capsuleAACube(static_cast(subShape), cubeCenter, cubeSide, collisions) || touching; + } + } + return touching; + } return false; } @@ -572,4 +590,62 @@ bool listList(const ListShape* listA, const ListShape* listB, CollisionList& col return touching; } +// helper function +bool sphereAACube(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(fabs(BA.x), fabs(BA.y)), fabs(BA.z)); + glm::vec3 surfaceB = cubeCenter - (0.5f * cubeSide / maxBA) * BA; + // collision happens when "vector to surfaceB from surfaceA" dots with BA to produce a positive value + glm::vec3 surfaceBA = surfaceB - surfaceA; + if (glm::dot(surfaceBA, BA) > 0.f) { + CollisionInfo* collision = collisions.getNewCollision(); + if (collision) { + collision->_penetration = surfaceBA; + // contactPoint is on surface of A + collision->_contactPoint = surfaceA; + 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; + return true; + } + } + return false; +} + +bool sphereAACube(const SphereShape* sphereA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) { + return sphereAACube(sphereA->getPosition(), sphereA->getRadius(), cubeCenter, cubeSide, collisions); +} + +bool capsuleAACube(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->getPosition(), capsuleAxis); + float halfHeight = capsuleA->getHalfHeight(); + if (offset > halfHeight) { + offset = halfHeight; + } else if (offset < -halfHeight) { + offset = -halfHeight; + } + glm::vec3 nearestApproach = capsuleA->getPosition() + offset * capsuleAxis; + // collide nearest approach like a sphere at that point + return sphereAACube(nearestApproach, capsuleA->getRadius(), cubeCenter, cubeSide, collisions); +} + + } // namespace ShapeCollider diff --git a/libraries/shared/src/ShapeCollider.h b/libraries/shared/src/ShapeCollider.h index a020050662..9e83e31571 100644 --- a/libraries/shared/src/ShapeCollider.h +++ b/libraries/shared/src/ShapeCollider.h @@ -12,7 +12,6 @@ #ifndef hifi_ShapeCollider_h #define hifi_ShapeCollider_h -#include "AABox.h" #include "CapsuleShape.h" #include "CollisionInfo.h" #include "ListShape.h" @@ -35,10 +34,11 @@ namespace ShapeCollider { bool collideShapesCoarse(const QVector& shapesA, const QVector& shapesB, CollisionInfo& collision); /// \param shapeA a pointer to a shape - /// \param boxB an axis aligned box + /// \param cubeCenter center of cube + /// \param cubeSide lenght of side of cube /// \param collisions[out] average collision details - /// \return true if shapeA collides with boxB - bool collideShapeWithBox(const Shape* shapeA, const AABox& boxB, CollisionList& collisions); + /// \return true if shapeA collides with axis aligned cube + bool collideShapeWithAACube(const Shape* shapeA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions); /// \param sphereA pointer to first shape /// \param sphereB pointer to second shape @@ -136,6 +136,20 @@ namespace ShapeCollider { /// \return true if shapes collide bool listList(const ListShape* listA, const ListShape* listB, CollisionList& collisions); + /// \param sphereA pointer to sphere + /// \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 sphereAACube(const SphereShape* sphereA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions); + + /// \param capsuleA pointer to capsule + /// \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 capsuleAACube(const CapsuleShape* capsuleA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions); + } // namespace ShapeCollider #endif // hifi_ShapeCollider_h