Adding ShapeColliderTests.*

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) {}
 

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<const SphereShape*>(shapeA);
         if (shapeB->getType() == Shape::SPHERE_SHAPE) {
             return sphereSphere(sphereA, static_cast<const SphereShape*>(shapeB), 
-                    rotationAB, offsetB, collision);
+                    rotationAB, offsetA, collision);
         } else if (shapeB->getType() == Shape::CAPSULE_SHAPE) {
             return sphereCapsule(sphereA, static_cast<const CapsuleShape*>(shapeB), 
-                    rotationAB, offsetB, collision);
+                    rotationAB, offsetA, collision);
         }
     } else if (shapeA->getType() == Shape::CAPSULE_SHAPE) {
         const CapsuleShape* capsuleA = static_cast<const CapsuleShape*>(shapeA);
         if (shapeB->getType() == Shape::SPHERE_SHAPE) {
             return capsuleSphere(capsuleA, static_cast<const SphereShape*>(shapeB), 
-                    rotationAB, offsetB, collision);
+                    rotationAB, offsetA, collision);
         } else if (shapeB->getType() == Shape::CAPSULE_SHAPE) {
             return capsuleCapsule(capsuleA, static_cast<const CapsuleShape*>(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;
 }
 

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
 

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; }

tests/physics/src/ShapeColliderTests.cpp

@@ -13,21 +13,42 @@
 #include <glm/gtx/quaternion.hpp>
 
 #include <CollisionInfo.h>
+#include <ShapeCollider.h>
 #include <SharedUtil.h>
+#include <SphereShape.h>
 
 #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();
 }

tests/physics/src/ShapeColliderTests.h

@@ -11,8 +11,8 @@
 
 namespace ShapeColliderTests {
 
-    //void rotateThenTranslate();
-    //void translateThenRotate();
+    void sphereSphere();
+    //void test2();
 
     void runAllTests(); 
 }