diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index 70f9b9c4bf..6ccf9df9c6 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -10,6 +10,7 @@ #include +#include "GeometryUtil.h" #include "ShapeCollider.h" // NOTE: @@ -19,7 +20,7 @@ namespace ShapeCollider { -bool shapeShape(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { +bool collideShapes(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { // ATM we only have two shape types so we just check every case. // TODO: make a fast lookup for correct method int typeA = shapeA->getType(); @@ -49,6 +50,30 @@ bool shapeShape(const Shape* shapeA, const Shape* shapeB, CollisionList& collisi return false; } +static CollisionList tempCollisions(32); + +bool collideShapesCoarse(const QVector& shapesA, const QVector& shapesB, CollisionInfo& collision) { + tempCollisions.clear(); + foreach (const Shape* shapeA, shapesA) { + foreach (const Shape* shapeB, shapesB) { + ShapeCollider::collideShapes(shapeA, shapeB, tempCollisions); + } + } + if (tempCollisions.size() > 0) { + glm::vec3 totalPenetration(0.f); + glm::vec3 averageContactPoint(0.f); + for (int j = 0; j < tempCollisions.size(); ++j) { + CollisionInfo* c = tempCollisions.getCollision(j); + totalPenetration = addPenetrations(totalPenetration, c->_penetration); + averageContactPoint += c->_contactPoint; + } + collision._penetration = totalPenetration; + collision._contactPoint = averageContactPoint / (float)(tempCollisions.size()); + return true; + } + return false; +} + bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, CollisionList& collisions) { glm::vec3 BA = sphereB->getPosition() - sphereA->getPosition(); float distanceSquared = glm::dot(BA, BA); @@ -407,7 +432,7 @@ bool listList(const ListShape* listA, const ListShape* listB, CollisionList& col 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 = shapeShape(subShape, listB->getSubShape(j), collisions) || touching; + touching = collideShapes(subShape, listB->getSubShape(j), collisions) || touching; } } return touching; diff --git a/libraries/shared/src/ShapeCollider.h b/libraries/shared/src/ShapeCollider.h index e3e044c8fe..841a607bc3 100644 --- a/libraries/shared/src/ShapeCollider.h +++ b/libraries/shared/src/ShapeCollider.h @@ -19,9 +19,15 @@ namespace ShapeCollider { /// \param shapeA pointer to first shape /// \param shapeB pointer to second shape - /// \param[out] collisions where to append collision details + /// \param collisions[out] collision details /// \return true if shapes collide - bool shapeShape(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions); + bool collideShapes(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions); + + /// \param shapesA list of shapes + /// \param shapeB list of shapes + /// \param collisions[out] average collision details + /// \return true if any shapes collide + bool collideShapesCoarse(const QVector& shapesA, const QVector& shapesB, CollisionInfo& collision); /// \param sphereA pointer to first shape /// \param sphereB pointer to second shape diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp index 7aeec84651..0960f7628a 100644 --- a/tests/physics/src/ShapeColliderTests.cpp +++ b/tests/physics/src/ShapeColliderTests.cpp @@ -40,7 +40,7 @@ void ShapeColliderTests::sphereMissesSphere() { // collide A to B... { - bool touching = ShapeCollider::shapeShape(&sphereA, &sphereB, collisions); + bool touching = ShapeCollider::collideShapes(&sphereA, &sphereB, collisions); if (touching) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphereA and sphereB should NOT touch" << std::endl; @@ -49,7 +49,7 @@ void ShapeColliderTests::sphereMissesSphere() { // collide B to A... { - bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collisions); + bool touching = ShapeCollider::collideShapes(&sphereB, &sphereA, collisions); if (touching) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphereA and sphereB should NOT touch" << std::endl; @@ -58,7 +58,7 @@ void ShapeColliderTests::sphereMissesSphere() { // also test shapeShape { - bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collisions); + bool touching = ShapeCollider::collideShapes(&sphereB, &sphereA, collisions); if (touching) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphereA and sphereB should NOT touch" << std::endl; @@ -90,7 +90,7 @@ void ShapeColliderTests::sphereTouchesSphere() { // collide A to B... { - bool touching = ShapeCollider::shapeShape(&sphereA, &sphereB, collisions); + bool touching = ShapeCollider::collideShapes(&sphereA, &sphereB, collisions); if (!touching) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphereA and sphereB should touch" << std::endl; @@ -133,7 +133,7 @@ void ShapeColliderTests::sphereTouchesSphere() { // collide B to A... { - bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collisions); + bool touching = ShapeCollider::collideShapes(&sphereB, &sphereA, collisions); if (!touching) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphereA and sphereB should touch" << std::endl; @@ -196,7 +196,7 @@ void ShapeColliderTests::sphereMissesCapsule() { sphereA.setPosition(rotation * localPosition + translation); // sphereA agains capsuleB - if (ShapeCollider::shapeShape(&sphereA, &capsuleB, collisions)) + if (ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere and capsule should NOT touch" @@ -204,7 +204,7 @@ void ShapeColliderTests::sphereMissesCapsule() { } // capsuleB against sphereA - if (ShapeCollider::shapeShape(&capsuleB, &sphereA, collisions)) + if (ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere and capsule should NOT touch" @@ -238,7 +238,7 @@ void ShapeColliderTests::sphereTouchesCapsule() { { // sphereA collides with capsuleB's cylindrical wall sphereA.setPosition(radialOffset * xAxis); - if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collisions)) + if (!ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere and capsule should touch" @@ -269,7 +269,7 @@ void ShapeColliderTests::sphereTouchesCapsule() { } // capsuleB collides with sphereA - if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collisions)) + if (!ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and sphere should touch" @@ -305,7 +305,7 @@ void ShapeColliderTests::sphereTouchesCapsule() { glm::vec3 axialOffset = (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis; sphereA.setPosition(axialOffset * yAxis); - if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collisions)) + if (!ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere and capsule should touch" @@ -336,7 +336,7 @@ void ShapeColliderTests::sphereTouchesCapsule() { } // capsuleB collides with sphereA - if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collisions)) + if (!ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and sphere should touch" @@ -372,7 +372,7 @@ void ShapeColliderTests::sphereTouchesCapsule() { glm::vec3 axialOffset = - (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis; sphereA.setPosition(axialOffset * yAxis); - if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collisions)) + if (!ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere and capsule should touch" @@ -403,7 +403,7 @@ void ShapeColliderTests::sphereTouchesCapsule() { } // capsuleB collides with sphereA - if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collisions)) + if (!ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and sphere should touch" @@ -459,13 +459,13 @@ void ShapeColliderTests::capsuleMissesCapsule() { // side by side capsuleB.setPosition((1.01f * totalRadius) * xAxis); - if (ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions)) + if (ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should NOT touch" << std::endl; } - if (ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions)) + if (ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should NOT touch" @@ -474,13 +474,13 @@ void ShapeColliderTests::capsuleMissesCapsule() { // end to end capsuleB.setPosition((1.01f * totalHalfLength) * xAxis); - if (ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions)) + if (ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should NOT touch" << std::endl; } - if (ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions)) + if (ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should NOT touch" @@ -491,13 +491,13 @@ void ShapeColliderTests::capsuleMissesCapsule() { glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis); capsuleB.setRotation(rotation); capsuleB.setPosition((1.01f * (totalRadius + capsuleB.getHalfHeight())) * xAxis); - if (ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions)) + if (ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should NOT touch" << std::endl; } - if (ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions)) + if (ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should NOT touch" @@ -529,7 +529,7 @@ void ShapeColliderTests::capsuleTouchesCapsule() { { // side by side capsuleB.setPosition((0.99f * totalRadius) * xAxis); - if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions)) + if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch" @@ -537,7 +537,7 @@ void ShapeColliderTests::capsuleTouchesCapsule() { } else { ++numCollisions; } - if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions)) + if (!ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch" @@ -550,7 +550,7 @@ void ShapeColliderTests::capsuleTouchesCapsule() { { // end to end capsuleB.setPosition((0.99f * totalHalfLength) * yAxis); - if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions)) + if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch" @@ -558,7 +558,7 @@ void ShapeColliderTests::capsuleTouchesCapsule() { } else { ++numCollisions; } - if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions)) + if (!ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch" @@ -573,7 +573,7 @@ void ShapeColliderTests::capsuleTouchesCapsule() { capsuleB.setRotation(rotation); capsuleB.setPosition((0.99f * (totalRadius + capsuleB.getHalfHeight())) * xAxis); - if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions)) + if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch" @@ -581,7 +581,7 @@ void ShapeColliderTests::capsuleTouchesCapsule() { } else { ++numCollisions; } - if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions)) + if (!ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch" @@ -599,7 +599,7 @@ void ShapeColliderTests::capsuleTouchesCapsule() { capsuleB.setPosition(positionB); // capsuleA vs capsuleB - if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions)) + if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch" @@ -628,7 +628,7 @@ void ShapeColliderTests::capsuleTouchesCapsule() { } // capsuleB vs capsuleA - if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions)) + if (!ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch" @@ -666,7 +666,7 @@ void ShapeColliderTests::capsuleTouchesCapsule() { capsuleB.setPosition(positionB); // capsuleA vs capsuleB - if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions)) + if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch"