diff --git a/libraries/shared/src/Shape.h b/libraries/shared/src/Shape.h index f3f3c2c74a..0e6d17c059 100644 --- a/libraries/shared/src/Shape.h +++ b/libraries/shared/src/Shape.h @@ -34,6 +34,7 @@ public: protected: Shape(Type type) : _type(type), _boundingRadius(0.f), _position(0.f), _rotation() {} + Shape(Type type, const glm::vec3& position, const glm::quat& rotation) : _type(type), _boundingRadius(0.f), _position(position), _rotation(rotation) {} diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index 0f3aabc51e..f3598ebf39 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -11,35 +11,35 @@ namespace ShapeCollider { bool shapeShape(const Shape* shapeA, const Shape* shapeB, - const glm::quat& rotationAB, const glm::vec3& offsetB, CollisionInfo& collision) { + const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision) { // ATM we only have two shape types so we just check every case. // TODO: make a fast lookup for correct method if (shapeA->getType() == Shape::SPHERE_SHAPE) { const SphereShape* sphereA = static_cast(shapeA); if (shapeB->getType() == Shape::SPHERE_SHAPE) { return sphereSphere(sphereA, static_cast(shapeB), - rotationAB, offsetB, collision); + rotationAB, offsetA, collision); } else if (shapeB->getType() == Shape::CAPSULE_SHAPE) { return sphereCapsule(sphereA, static_cast(shapeB), - rotationAB, offsetB, collision); + rotationAB, offsetA, collision); } } else if (shapeA->getType() == Shape::CAPSULE_SHAPE) { const CapsuleShape* capsuleA = static_cast(shapeA); if (shapeB->getType() == Shape::SPHERE_SHAPE) { return capsuleSphere(capsuleA, static_cast(shapeB), - rotationAB, offsetB, collision); + rotationAB, offsetA, collision); } else if (shapeB->getType() == Shape::CAPSULE_SHAPE) { return capsuleCapsule(capsuleA, static_cast(shapeB), - rotationAB, offsetB, collision); + rotationAB, offsetA, collision); } } return false; } bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, - const glm::quat& rotationAB, const glm::vec3& offsetB, CollisionInfo& collision) { + const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision) { // A in B's frame - glm::vec3 A = offsetB + rotationAB * sphereA->getPosition(); + glm::vec3 A = rotationAB * sphereA->getPosition() + offsetA; // BA = B - A = from A to B, in B's frame glm::vec3 BA = sphereB->getPosition() - A; float distanceSquared = glm::dot(BA, BA); @@ -119,10 +119,10 @@ bool sphereCapsuleHelper(float sphereRadius, const glm::vec3 sphereCenter, } bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, - const glm::quat& rotationAB, const glm::vec3& offsetB, CollisionInfo& collision) { + const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision) { // transform sphereA all the way to capsuleB's natural frame glm::quat rotationB = capsuleB->getRotation(); - glm::vec3 sphereCenter = rotationB * (offsetB + rotationAB * sphereA->getPosition() - capsuleB->getPosition()); + glm::vec3 sphereCenter = rotationB * (offsetA + rotationAB * sphereA->getPosition() - capsuleB->getPosition()); if (sphereCapsuleHelper(sphereA->getRadius(), sphereCenter, capsuleB, collision)) { // need to transform collision details back into B's offset frame collision.rotateThenTranslate(glm::inverse(rotationB), capsuleB->getPosition()); @@ -132,24 +132,24 @@ bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, } bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, - const glm::quat& rotationAB, const glm::vec3& offsetB, CollisionInfo& collision) { + const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision) { // transform sphereB all the way to capsuleA's natural frame glm::quat rotationBA = glm::inverse(rotationAB); glm::quat rotationA = capsuleA->getRotation(); - glm::vec3 offsetA = rotationBA * (-offsetB); - glm::vec3 sphereCenter = rotationA * (offsetA + rotationBA * sphereB->getPosition() - capsuleA->getPosition()); + glm::vec3 offsetB = rotationBA * (-offsetA); + glm::vec3 sphereCenter = rotationA * (offsetB + rotationBA * sphereB->getPosition() - capsuleA->getPosition()); if (sphereCapsuleHelper(sphereB->getRadius(), sphereCenter, capsuleA, collision)) { // need to transform collision details back into B's offset frame // BUG: these back translations are probably not right collision.rotateThenTranslate(glm::inverse(rotationA), capsuleA->getPosition()); - collision.rotateThenTranslate(glm::inverse(rotationAB), -offsetB); + collision.rotateThenTranslate(glm::inverse(rotationAB), -offsetA); return true; } return false; } bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, - const glm::quat& rotationAB, const glm::vec3& offsetB, CollisionInfo& collision) { + const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision) { return false; } diff --git a/libraries/shared/src/ShapeCollider.h b/libraries/shared/src/ShapeCollider.h index f6505707fa..4cef6f943f 100644 --- a/libraries/shared/src/ShapeCollider.h +++ b/libraries/shared/src/ShapeCollider.h @@ -19,47 +19,47 @@ namespace ShapeCollider { /// \param shapeA pointer to first shape /// \param shapeB pointer to second shape /// \param rotationAB rotation from A into reference frame of B -/// \param offsetB offset of B (in B's frame) +/// \param offsetA offset of A (in B's frame) /// \param[out] collision where to store collision details /// \return true of shapes collide bool shapeShape(const Shape* shapeA, const Shape* shapeB, - const glm::quat& rotationAB, const glm::vec3& offsetB, CollisionInfo& collision); + const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision); /// \param sphereA pointer to first shape (sphere) /// \param sphereB pointer to second shape (sphere) /// \param rotationAB rotation from A into reference frame of B -/// \param offsetB offset of B (in B's frame) +/// \param offsetA offset of A (in B's frame) /// \param[out] collision where to store collision details /// \return true of shapes collide bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, - const glm::quat& rotationAB, const glm::vec3& offsetB, CollisionInfo& collision); + const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision); /// \param sphereA pointer to first shape (sphere) /// \param capsuleB pointer to second shape (capsule) /// \param rotationAB rotation from A into reference frame of B -/// \param offsetB offset of B (in B's frame) +/// \param offsetA offset of A (in B's frame) /// \param[out] collision where to store collision details /// \return true of shapes collide bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, - const glm::quat& rotationAB, const glm::vec3& offsetB, CollisionInfo& collision); + const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision); /// \param capsuleA pointer to first shape (capsule) /// \param sphereB pointer to second shape (sphere) /// \param rotationAB rotation from A into reference frame of B -/// \param offsetB offset of B (in B's frame) +/// \param offsetA offset of A (in B's frame) /// \param[out] collision where to store collision details /// \return true of shapes collide bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, - const glm::quat& rotationAB, const glm::vec3& offsetB, CollisionInfo& collision); + const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision); /// \param capsuleA pointer to first shape (capsule) /// \param capsuleB pointer to second shape (capsule) /// \param rotationAB rotation from A into reference frame of B -/// \param offsetB offset of B (in B's frame) +/// \param offsetA offset of A (in B's frame) /// \param[out] collision where to store collision details /// \return true of shapes collide bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, - const glm::quat& rotationAB, const glm::vec3& offsetB, CollisionInfo& collision); + const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision); } // namespace ShapeCollider diff --git a/libraries/shared/src/SphereShape.h b/libraries/shared/src/SphereShape.h index 879fcb3ff8..a1028956ee 100644 --- a/libraries/shared/src/SphereShape.h +++ b/libraries/shared/src/SphereShape.h @@ -15,6 +15,11 @@ class SphereShape : public Shape { public: SphereShape() : Shape(Shape::SPHERE_SHAPE) {} + SphereShape(float radius, const glm::vec3& position) : Shape(Shape::SPHERE_SHAPE) { + _boundingRadius = radius; + setPosition(position); + } + float getRadius() const { return _boundingRadius; } void setRadius(float radius) { _boundingRadius = radius; } diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp index c103179b07..b964e2f6de 100644 --- a/tests/physics/src/ShapeColliderTests.cpp +++ b/tests/physics/src/ShapeColliderTests.cpp @@ -13,21 +13,42 @@ #include #include +#include #include +#include #include "PhysicsTestUtil.h" #include "ShapeColliderTests.h" +const glm::vec3 origin(0.f); -/* -void ShapeColliderTests::test1() { +void ShapeColliderTests::sphereSphere() { + CollisionInfo collision; + + float radius = 2.f; + SphereShape sphereA(radius, origin); + SphereShape sphereB(radius, origin); + + glm::vec3 translation(0.5f * radius, 0.f, 0.f); + glm::quat rotation; + + // these spheres should be touching + bool touching = ShapeCollider::sphereSphere(&sphereA, &sphereB, rotation, translation, collision); + if (!touching) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphereA and sphereB do not touch" << std::endl; + } + + // TODO: verify the collision info is good... + // penetration should point from A into B } +/* void ShapeColliderTests::test2() { } */ void ShapeColliderTests::runAllTests() { -// ShapeColliderTests::test1(); + ShapeColliderTests::sphereSphere(); // ShapeColliderTests::test2(); } diff --git a/tests/physics/src/ShapeColliderTests.h b/tests/physics/src/ShapeColliderTests.h index a51fb83854..a3a19805f8 100644 --- a/tests/physics/src/ShapeColliderTests.h +++ b/tests/physics/src/ShapeColliderTests.h @@ -11,8 +11,8 @@ namespace ShapeColliderTests { - //void rotateThenTranslate(); - //void translateThenRotate(); + void sphereSphere(); + //void test2(); void runAllTests(); }