Adding CapsuleSphere collisions with tests.

libraries/shared/src/CapsuleShape.cpp

@@ -6,6 +6,7 @@
 //  Copyright (c) 2014 High Fidelity, Inc. All rights reserved.
 //
 
+#include <iostream>
 #include <glm/gtx/vector_angle.hpp>
 
 #include "CapsuleShape.h"
@@ -13,6 +14,7 @@
 
 
 // default axis of CapsuleShape is Y-axis
+const glm::vec3 localAxis(0.f, 1.f, 0.f);
 
 CapsuleShape::CapsuleShape() : Shape(Shape::CAPSULE_SHAPE) {}
 
@@ -43,6 +45,21 @@ CapsuleShape::CapsuleShape(float radius, const glm::vec3& startPoint, const glm:
     updateBoundingRadius();
 }
 
+/// \param[out] startPoint is the center of start cap
+void CapsuleShape::getStartPoint(glm::vec3& startPoint) const {
+    startPoint = getPosition() - _halfHeight * (_rotation * glm::vec3(0.f, 1.f, 0.f));
+}
+
+/// \param[out] endPoint is the center of the end cap
+void CapsuleShape::getEndPoint(glm::vec3& endPoint) const {
+    endPoint = getPosition() + _halfHeight * (_rotation * glm::vec3(0.f, 1.f, 0.f));
+}
+
+void CapsuleShape::computeNormalizedAxis(glm::vec3& axis) const {
+    // default axis of a capsule is along the yAxis
+    axis = _rotation * glm::vec3(0.f, 1.f, 0.f);
+}
+
 void CapsuleShape::setRadius(float radius) {
     _radius = radius;
     updateBoundingRadius();

libraries/shared/src/CapsuleShape.h

@@ -11,6 +11,7 @@
 
 #include "Shape.h"
 
+// adebug bookmark TODO: convert to new world-frame approach
 // default axis of CapsuleShape is Y-axis
 
 class CapsuleShape : public Shape {
@@ -23,6 +24,14 @@ public:
     float getRadius() const { return _radius; }
     float getHalfHeight() const { return _halfHeight; }
 
+    /// \param[out] startPoint is the center of start cap
+    void getStartPoint(glm::vec3& startPoint) const;
+
+    /// \param[out] endPoint is the center of the end cap
+    void getEndPoint(glm::vec3& endPoint) const;
+
+    void computeNormalizedAxis(glm::vec3& axis) const;
+
     void setRadius(float radius);
     void setHalfHeight(float height);
     void setRadiusAndHalfHeight(float radius, float height);

libraries/shared/src/CollisionInfo.cpp

@@ -8,17 +8,6 @@
 
 #include "CollisionInfo.h"
 
-void CollisionInfo::rotateThenTranslate(const glm::quat& rotation, const glm::vec3& translation) {
-    _contactPoint = translation + rotation * _contactPoint;
-    _penetration = rotation * _penetration;
-    _addedVelocity = rotation * _addedVelocity;
-}
-
-void CollisionInfo::translateThenRotate(const glm::vec3& translation, const glm::quat& rotation) {
-    _contactPoint = rotation * (_contactPoint + translation);
-    _penetration = rotation * _penetration;
-    _addedVelocity = rotation * _addedVelocity;
-}
 
 CollisionList::CollisionList(int maxSize) :
     _maxSize(maxSize),

libraries/shared/src/CollisionInfo.h

@@ -56,12 +56,6 @@ public:
 
     ~CollisionInfo() {}
 
-    /// Rotate and translate the details.
-    void rotateThenTranslate(const glm::quat& rotation, const glm::vec3& translation);
-
-    /// Translate then rotate the details
-    void translateThenRotate(const glm::vec3& translation, const glm::quat& rotation);
-
     qint32 _type;             // type of Collision (will determine what is supposed to be in _data and _flags)
     void* _data;              // pointer to user supplied data
     quint32 _flags;           // 32 bits for whatever

libraries/shared/src/Shape.h

@@ -1,6 +1,5 @@
 //
 //  Shape.h
-//  hifi
 //
 //  Created by Andrew Meadows on 2014.02.20
 //  Copyright (c) 2014 High Fidelity, Inc. All rights reserved.
@@ -20,6 +19,7 @@ public:
         SPHERE_SHAPE,
         CAPSULE_SHAPE,
         BOX_SHAPE,
+        LIST_SHAPE
     };
 
     Shape() : _type(UNKNOWN_SHAPE), _boundingRadius(0.f), _position(0.f), _rotation() { }
@@ -33,8 +33,12 @@ public:
     void setRotation(const glm::quat& rotation) { _rotation = rotation; }
 
 protected:
+    // these ctors are protected (used by derived classes only)
     Shape(Type type) : _type(type), _boundingRadius(0.f), _position(0.f), _rotation() {}
 
+    Shape(Type type, const glm::vec3& position) 
+        : _type(type), _boundingRadius(0.f), _position(position), _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

@@ -6,42 +6,37 @@
 //  Copyright (c) 2014 High Fidelity, Inc. All rights reserved.
 //
 
+#include <iostream>
+
+#include <glm/gtx/norm.hpp>
+
 #include "ShapeCollider.h"
 
 namespace ShapeCollider {
 
-bool shapeShape(const Shape* shapeA, const Shape* shapeB, 
+bool shapeShape(const Shape* shapeA, const Shape* shapeB, 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), 
+            return sphereSphere(sphereA, static_cast<const SphereShape*>(shapeB), collision);
-                    rotationAB, offsetA, collision);
         } else if (shapeB->getType() == Shape::CAPSULE_SHAPE) {
-            return sphereCapsule(sphereA, static_cast<const CapsuleShape*>(shapeB), 
+            return sphereCapsule(sphereA, static_cast<const CapsuleShape*>(shapeB), 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), 
+            return capsuleSphere(capsuleA, static_cast<const SphereShape*>(shapeB), collision);
-                    rotationAB, offsetA, collision);
         } else if (shapeB->getType() == Shape::CAPSULE_SHAPE) {
-            return capsuleCapsule(capsuleA, static_cast<const CapsuleShape*>(shapeB), 
+            return capsuleCapsule(capsuleA, static_cast<const CapsuleShape*>(shapeB), collision);
-                    rotationAB, offsetA, collision);
         }
     }
     return false;
 }
 
-bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, 
+bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, CollisionInfo& collision) {
-        const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision) {
+    glm::vec3 BA = sphereB->getPosition() - sphereA->getPosition();
-    // A in B's frame
-    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);
     float totalRadius = sphereA->getRadius() + sphereB->getRadius();
     if (distanceSquared < totalRadius * totalRadius) {
@@ -53,103 +48,124 @@ bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB,
         } else {
             BA /= distance;
         }
-        // store the collision details in B's frame
+        // penetration points from A into B
         collision._penetration = BA * (totalRadius - distance);
-        collision._contactPoint = A + sphereA->getRadius() * BA;
+        // contactPoint is on surface of A
+        collision._contactPoint = sphereA->getPosition() + sphereA->getRadius() * BA;
         return true;
     }
     return false;
 }
 
-// everything in the capsule's natural frame (where its axis is along yAxis)
+bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, CollisionInfo& collision) {
-bool sphereCapsuleHelper(float sphereRadius, const glm::vec3 sphereCenter, 
+    // find sphereA's closest approach to axis of capsuleB
-        const CapsuleShape* capsule, CollisionInfo& collision) {
+    glm::vec3 BA = capsuleB->getPosition() - sphereA->getPosition();
-    float xzSquared = sphereCenter.x * sphereCenter.x + sphereCenter.y * sphereCenter.y;
+    glm::vec3 capsuleAxis; 
-    float totalRadius = sphereRadius + capsule->getRadius();
+    capsuleB->computeNormalizedAxis(capsuleAxis);
-    if (xzSquared < totalRadius * totalRadius) {
+    float axialDistance = - glm::dot(BA, capsuleAxis);
-        float fabsY = fabs(sphereCenter.y);
+    float absAxialDistance = fabs(axialDistance);
-        float halfHeight = capsule->getHalfHeight();
+    float totalRadius = sphereA->getRadius() + capsuleB->getRadius();
-        if (fabsY < halfHeight) {
+    if (absAxialDistance < totalRadius + capsuleB->getHalfHeight()) {
-            // sphere collides with cylindrical wall
+        glm::vec3 radialAxis = BA + axialDistance * capsuleAxis; // points from A to axis of B
-            glm::vec3 BA = -sphereCenter;
+        float radialDistance2 = glm::length2(radialAxis);
-            BA.y = 0.f;
+        if (radialDistance2 > totalRadius * totalRadius) {
-            float distance = sqrtf(xzSquared);
+            // sphere is too far from capsule axis
-            if (distance < EPSILON) {
+            return false;
-                // for now we don't handle this singular case
+        }
+        if (absAxialDistance > capsuleB->getHalfHeight()) {
+            // sphere hits capsule on a cap --> recompute radialAxis to point from spherA to cap center
+            float sign = (axialDistance > 0.f) ? 1.f : -1.f;
+            radialAxis = BA + (sign * capsuleB->getHalfHeight()) * capsuleAxis;
+            radialDistance2 = glm::length2(radialAxis);
+        }
+        if (radialDistance2 > EPSILON * EPSILON) {
+            // 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->getPosition() + sphereA->getRadius() * radialAxis;
+        } 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;
             }
-            BA /= distance;
+            // ... but still defined for the cap case
-            collision._penetration = BA * (totalRadius - distance);
+            if (axialDistance < 0.f) {
-            collision._contactPoint = BA * (sphereRadius - totalRadius + distance);
+                // we're hitting the start cap, so we negate the capsuleAxis
-            collision._contactPoint.y = fabsY;
+                capsuleAxis *= -1;
-            if (sphereCenter.y < 0.f) {
-                // negate the y elements of the collision info
-                collision._penetration.y *= -1.f;
-                collision._contactPoint.y *= -1.f;
             }
-            return true;
+            // penetration points from A into B
+            float sign = (axialDistance > 0.f) ? -1.f : 1.f;
+            collision._penetration = (sign * (totalRadius + capsuleB->getHalfHeight() - absAxialDistance)) * capsuleAxis;
+            // contactPoint is on surface of sphereA
+            collision._contactPoint = sphereA->getPosition() + (sign * sphereA->getRadius()) * capsuleAxis;
+        }
+        return true;
+    }
+    return false;
+}
+
+bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, CollisionInfo& collision) {
+    // find sphereB's closest approach to axis of capsuleA
+    glm::vec3 AB = capsuleA->getPosition() - sphereB->getPosition();
+    glm::vec3 capsuleAxis;
+    capsuleA->computeNormalizedAxis(capsuleAxis);
+    float axialDistance = - glm::dot(AB, capsuleAxis);
+    float absAxialDistance = fabs(axialDistance);
+    float totalRadius = sphereB->getRadius() + capsuleA->getRadius();
+    if (absAxialDistance < totalRadius + capsuleA->getHalfHeight()) {
+        glm::vec3 radialAxis = AB + axialDistance * capsuleAxis; // from sphereB to axis of capsuleA
+        float radialDistance2 = glm::length2(radialAxis);
+        if (radialDistance2 > totalRadius * totalRadius) {
+            // sphere is too far from capsule axis
+            return false;
+        }
+
+        // closestApproach = point on capsuleA's axis that is closest to sphereB's center
+        glm::vec3 closestApproach = capsuleA->getPosition() + axialDistance * capsuleAxis;
+
+        if (absAxialDistance > capsuleA->getHalfHeight()) {
+            // sphere hits capsule on a cap 
+            // --> recompute radialAxis and closestApproach
+            float sign = (axialDistance > 0.f) ? 1.f : -1.f;
+            closestApproach = capsuleA->getPosition() + (sign * capsuleA->getHalfHeight()) * capsuleAxis;
+            radialAxis = closestApproach - sphereB->getPosition();
+            radialDistance2 = glm::length2(radialAxis);
+        }
+        if (radialDistance2 > EPSILON * EPSILON) {
+            // normalize the radialAxis
+            float radialDistance = sqrtf(radialDistance2);
+            radialAxis /= radialDistance;
+            // penetration points from A into B
+            collision._penetration = (radialDistance - totalRadius) * radialAxis; // points from A into B
+            // contactPoint is on surface of capsuleA
+            collision._contactPoint = closestApproach - capsuleA->getRadius() * radialAxis;
         } else {
-            // tansform into frame where cap is at origin
+            // A is on B's axis, so the penetration is undefined... 
-            float newY = fabsY - halfHeight;
+            if (absAxialDistance > capsuleA->getHalfHeight()) {
-            float distance = sqrtf(newY * newY + xzSquared);
+                // ...for the cylinder case (for now we pretend the collision doesn't exist)
-            if (distance < totalRadius) {
+                return false;
-                // sphere collides with cap
+            } else {
-
+                // ... but still defined for the cap case
-                // BA points from sphere to cap
+                if (axialDistance < 0.f) {
-                glm::vec3 BA(-sphereCenter.x, newY, -sphereCenter.z);
+                    // we're hitting the start cap, so we negate the capsuleAxis
-                if (distance < EPSILON) {
+                    capsuleAxis *= -1;
-                    // for now we don't handle this singular case
-                    return false;
                 }
-                BA /= distance;
+                float sign = (axialDistance > 0.f) ? 1.f : -1.f;
-                collision._penetration = BA * (totalRadius - distance);
+                collision._penetration = (sign * (totalRadius + capsuleA->getHalfHeight() - absAxialDistance)) * capsuleAxis;
-                collision._contactPoint = BA * (sphereRadius - totalRadius + distance);
+                // contactPoint is on surface of sphereA
-                collision._contactPoint.y += halfHeight;
+                collision._contactPoint = closestApproach + (sign * capsuleA->getRadius()) * capsuleAxis;
-
-                if (sphereCenter.y < 0.f) {
-                    // negate the y elements of the collision info
-                    collision._penetration.y *= -1.f;
-                    collision._contactPoint.y *= -1.f;
-                }
-                return true;
             }
         }
-    }
-    return false;
-}
-
-bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB,
-        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 * (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());
         return true;
     }
     return false;
 }
 
-bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB,
+bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, 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 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), -offsetA);
-        return true;
-    }
-    return false;
-}
-
-bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB,
-        const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision) {
     return false;
 }
 

libraries/shared/src/ShapeCollider.h

@@ -16,50 +16,35 @@
 
 namespace ShapeCollider {
 
-/// \param shapeA pointer to first shape
+    /// \param shapeA pointer to first shape
-/// \param shapeB pointer to second shape
+    /// \param shapeB pointer to second shape
-/// \param rotationAB rotation from A into reference frame of B
+    /// \param[out] collision where to store collision details
-/// \param offsetA offset of A (in B's frame)
+    /// \return true of shapes collide
-/// \param[out] collision where to store collision details
+    bool shapeShape(const Shape* shapeA, const Shape* shapeB, CollisionInfo& collision);
-/// \return true of shapes collide
-bool shapeShape(const Shape* shapeA, const Shape* shapeB, 
-        const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision);
 
-/// \param sphereA pointer to first shape (sphere)
+    /// \param sphereA pointer to first shape (sphere)
-/// \param sphereB pointer to second shape (sphere)
+    /// \param sphereB pointer to second shape (sphere)
-/// \param rotationAB rotation from A into reference frame of B
+    /// \param[out] collision where to store collision details
-/// \param offsetA offset of A (in B's frame)
+    /// \return true of shapes collide
-/// \param[out] collision where to store collision details
+    bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, CollisionInfo& collision);
-/// \return true of shapes collide
-bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, 
-        const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision);
 
-/// \param sphereA pointer to first shape (sphere)
+    /// \param sphereA pointer to first shape (sphere)
-/// \param capsuleB pointer to second shape (capsule)
+    /// \param capsuleB pointer to second shape (capsule)
-/// \param rotationAB rotation from A into reference frame of B
+    /// \param[out] collision where to store collision details
-/// \param offsetA offset of A (in B's frame)
+    /// \return true of shapes collide
-/// \param[out] collision where to store collision details
+    bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, CollisionInfo& collision);
-/// \return true of shapes collide
-bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB,
-        const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision);
 
-/// \param capsuleA pointer to first shape (capsule)
+    /// \param capsuleA pointer to first shape (capsule)
-/// \param sphereB pointer to second shape (sphere)
+    /// \param sphereB pointer to second shape (sphere)
-/// \param rotationAB rotation from A into reference frame of B
+    /// \param[out] collision where to store collision details
-/// \param offsetA offset of A (in B's frame)
+    /// \return true of shapes collide
-/// \param[out] collision where to store collision details
+    bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, CollisionInfo& collision);
-/// \return true of shapes collide
-bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB,
-        const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision);
 
-/// \param capsuleA pointer to first shape (capsule)
+    /// \param capsuleA pointer to first shape (capsule)
-/// \param capsuleB pointer to second shape (capsule)
+    /// \param capsuleB pointer to second shape (capsule)
-/// \param rotationAB rotation from A into reference frame of B
+    /// \param[out] collision where to store collision details
-/// \param offsetA offset of A (in B's frame)
+    /// \return true of shapes collide
-/// \param[out] collision where to store collision details
+    bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, CollisionInfo& collision);
-/// \return true of shapes collide
-bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB,
-        const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision);
 
 }   // namespace ShapeCollider
 

libraries/shared/src/SphereShape.h

@@ -15,9 +15,12 @@ class SphereShape : public Shape {
 public:
     SphereShape() : Shape(Shape::SPHERE_SHAPE) {}
 
-    SphereShape(float radius, const glm::vec3& position) : Shape(Shape::SPHERE_SHAPE) {
+    SphereShape(float radius) : Shape(Shape::SPHERE_SHAPE) {
+        _boundingRadius = radius;
+    }
+
+    SphereShape(float radius, const glm::vec3& position) : Shape(Shape::SPHERE_SHAPE, position) {
         _boundingRadius = radius;
-        setPosition(position);
     }
 
     float getRadius() const { return _boundingRadius; }

tests/physics/src/CollisionInfoTests.cpp

@@ -18,6 +18,7 @@
 #include "PhysicsTestUtil.h"
 
 
+/*
 void CollisionInfoTests::rotateThenTranslate() {
     CollisionInfo collision;
     collision._penetration = xAxis;
@@ -95,8 +96,9 @@ void CollisionInfoTests::translateThenRotate() {
             << std::endl;
     } 
 }
+*/
 
 void CollisionInfoTests::runAllTests() {
-    CollisionInfoTests::rotateThenTranslate();
+//    CollisionInfoTests::rotateThenTranslate();
-    CollisionInfoTests::translateThenRotate();
+//    CollisionInfoTests::translateThenRotate();
 }

tests/physics/src/CollisionInfoTests.h

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

tests/physics/src/ShapeColliderTests.cpp

@@ -22,33 +22,384 @@
 
 const glm::vec3 origin(0.f);
 
-void ShapeColliderTests::sphereSphere() {
+void ShapeColliderTests::sphereMissesSphere() {
+    // non-overlapping spheres of unequal size
+    float radiusA = 7.f;
+    float radiusB = 3.f;
+    float alpha = 1.2f;
+    float beta = 1.3f;
+    glm::vec3 offsetDirection = glm::normalize(glm::vec3(1.f, 2.f, 3.f));
+    float offsetDistance = alpha * radiusA + beta * radiusB;
+    float expectedPenetrationDistance = 0.f;
+
+    SphereShape sphereA(radiusA, origin);
+    SphereShape sphereB(radiusB, offsetDistance * offsetDirection);
     CollisionInfo collision;
 
-    float radius = 2.f;
+    // collide A to B...
-    SphereShape sphereA(radius, origin);
+    {
-    SphereShape sphereB(radius, origin);
+        bool touching = ShapeCollider::shapeShape(&sphereA, &sphereB, collision);
-
+        if (touching) {
-    glm::vec3 translation(0.5f * radius, 0.f, 0.f);
+            std::cout << __FILE__ << ":" << __LINE__
-    glm::quat rotation;
+                << " ERROR: sphereA and sphereB should NOT touch" << std::endl;
-
+        }
-    // 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...
+    // collide B to A...
-    // penetration should point from A into B
+    {
+        bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collision);
+        if (touching) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: sphereA and sphereB should NOT touch" << std::endl;
+        }
+    }
+
+    // also test shapeShape
+    {
+        bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collision);
+        if (touching) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: sphereA and sphereB should NOT touch" << std::endl;
+        }
+    }
 }
 
-/*
+void ShapeColliderTests::sphereTouchesSphere() {
-void ShapeColliderTests::test2() {
+    // overlapping spheres of unequal size
+    float radiusA = 7.f;
+    float radiusB = 3.f;
+    float alpha = 0.2f;
+    float beta = 0.3f;
+    glm::vec3 offsetDirection = glm::normalize(glm::vec3(1.f, 2.f, 3.f));
+    float offsetDistance = alpha * radiusA + beta * radiusB;
+    float expectedPenetrationDistance = (1.f - alpha) * radiusA + (1.f - beta) * radiusB;
+    glm::vec3 expectedPenetration = expectedPenetrationDistance * offsetDirection;
+
+    SphereShape sphereA(radiusA, origin);
+    SphereShape sphereB(radiusB, offsetDistance * offsetDirection);
+    CollisionInfo collision;
+
+    // collide A to B...
+    {
+        bool touching = ShapeCollider::shapeShape(&sphereA, &sphereB, collision);
+        if (!touching) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: sphereA and sphereB should touch" << std::endl;
+        }
+    
+        // penetration points from sphereA into sphereB
+        float inaccuracy = glm::length(collision._penetration - expectedPenetration);
+        if (fabs(inaccuracy) > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: bad penetration: expected = " << expectedPenetration
+                << " actual = " << collision._penetration 
+                << std::endl;
+        }
+    
+        // contactPoint is on surface of sphereA
+        glm::vec3 AtoB = sphereB.getPosition() - sphereA.getPosition();
+        glm::vec3 expectedContactPoint = sphereA.getPosition() + radiusA * glm::normalize(AtoB);
+        inaccuracy = glm::length(collision._contactPoint - expectedContactPoint);
+        if (fabs(inaccuracy) > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: bad contactPoint: expected = " << expectedContactPoint
+                << " actual = " << collision._contactPoint 
+                << std::endl;
+        }
+    }
+
+    // collide B to A...
+    {
+        bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collision);
+        if (!touching) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: sphereA and sphereB should touch" << std::endl;
+        }
+    
+        // penetration points from sphereA into sphereB
+        float inaccuracy = glm::length(collision._penetration + expectedPenetration);
+        if (fabs(inaccuracy) > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: bad penetration: expected = " << expectedPenetration
+                << " actual = " << collision._penetration 
+                << std::endl;
+        }
+    
+        // contactPoint is on surface of sphereA
+        glm::vec3 BtoA = sphereA.getPosition() - sphereB.getPosition();
+        glm::vec3 expectedContactPoint = sphereB.getPosition() + radiusB * glm::normalize(BtoA);
+        inaccuracy = glm::length(collision._contactPoint - expectedContactPoint);
+        if (fabs(inaccuracy) > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: bad contactPoint: expected = " << expectedContactPoint
+                << " actual = " << collision._contactPoint 
+                << std::endl;
+        }
+    }
+}
+
+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.8;
+    glm::vec3 axis = glm::normalize( glm::vec3(-7.f, 2.8f, 9.3f) );
+    glm::quat rotation = glm::angleAxis(angle, axis);
+    glm::vec3 translation(15.1f, -27.1f, -38.6f);
+    capsuleB.setRotation(rotation);
+    capsuleB.setPosition(translation);
+
+    // walk sphereA along the local yAxis next to, but not touching, capsuleB
+    glm::vec3 localStartPosition(radialOffset, axialOffset, 0.f);
+    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.setPosition(rotation * localPosition + translation);
+
+        CollisionInfo collision;
+        // sphereA agains capsuleB
+        if (ShapeCollider::shapeShape(&sphereA, &capsuleB, collision))
+        {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: sphere and capsule should NOT touch"
+                << std::endl;
+        }
+
+        // capsuleB against sphereA
+        if (ShapeCollider::shapeShape(&capsuleB, &sphereA, collision))
+        {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: sphere and capsule should NOT touch"
+                << std::endl;
+        }
+    }
+}
+
+void ShapeColliderTests::sphereTouchesCapsule() {
+    // overlapping sphere and capsule
+    float radiusA = 2.f;
+    float radiusB = 1.f;
+    float totalRadius = radiusA + radiusB;
+    float halfHeightB = 2.f;
+    float alpha = 0.5f;
+    float beta = 0.5f;
+    float axialOffset = 0.f;
+    float radialOffset = alpha * radiusA + beta * radiusB;
+    
+    SphereShape sphereA(radiusA);
+    CapsuleShape capsuleB(radiusB, halfHeightB);
+
+    CollisionInfo collision;
+    {   // sphereA collides with capsuleB's cylindrical wall
+        sphereA.setPosition(radialOffset * xAxis);
+
+        if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collision))
+        {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: sphere and capsule should touch"
+                << std::endl;
+        }
+    
+        // penetration points from sphereA into capsuleB
+        glm::vec3 expectedPenetration = (radialOffset - totalRadius) * xAxis;
+        float inaccuracy = glm::length(collision._penetration - expectedPenetration);
+        if (fabs(inaccuracy) > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: bad penetration: expected = " << expectedPenetration
+                << " actual = " << collision._penetration 
+                << std::endl;
+        }
+    
+        // contactPoint is on surface of sphereA
+        glm::vec3 expectedContactPoint = sphereA.getPosition() - radiusA * xAxis;
+        inaccuracy = glm::length(collision._contactPoint - expectedContactPoint);
+        if (fabs(inaccuracy) > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: bad contactPoint: expected = " << expectedContactPoint
+                << " actual = " << collision._contactPoint 
+                << std::endl;
+        }
+
+        // capsuleB collides with sphereA
+        if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collision))
+        {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: capsule and sphere should touch"
+                << std::endl;
+        }
+    
+        // penetration points from sphereA into capsuleB
+        expectedPenetration = - (radialOffset - totalRadius) * xAxis;
+        inaccuracy = glm::length(collision._penetration - expectedPenetration);
+        if (fabs(inaccuracy) > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: bad penetration: expected = " << expectedPenetration
+                << " actual = " << collision._penetration 
+                << std::endl;
+        }
+    
+        // contactPoint is on surface of capsuleB
+        glm::vec3 BtoA = sphereA.getPosition() - capsuleB.getPosition();
+        glm::vec3 closestApproach = capsuleB.getPosition() + glm::dot(BtoA, yAxis) * yAxis;
+        expectedContactPoint = closestApproach + radiusB * glm::normalize(BtoA - closestApproach);
+        inaccuracy = glm::length(collision._contactPoint - expectedContactPoint);
+        if (fabs(inaccuracy) > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: bad contactPoint: expected = " << expectedContactPoint
+                << " actual = " << collision._contactPoint 
+                << std::endl;
+        }
+    }
+    {   // sphereA hits end cap at axis
+        glm::vec3 axialOffset = (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis;
+        sphereA.setPosition(axialOffset * yAxis);
+        
+        if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collision))
+        {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: sphere and capsule should touch"
+                << std::endl;
+        }
+    
+        // penetration points from sphereA into capsuleB
+        glm::vec3 expectedPenetration = - ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis;
+        float inaccuracy = glm::length(collision._penetration - expectedPenetration);
+        if (fabs(inaccuracy) > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: bad penetration: expected = " << expectedPenetration
+                << " actual = " << collision._penetration 
+                << std::endl;
+        }
+    
+        // contactPoint is on surface of sphereA
+        glm::vec3 expectedContactPoint = sphereA.getPosition() - radiusA * yAxis;
+        inaccuracy = glm::length(collision._contactPoint - expectedContactPoint);
+        if (fabs(inaccuracy) > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: bad contactPoint: expected = " << expectedContactPoint
+                << " actual = " << collision._contactPoint 
+                << std::endl;
+        }
+
+        // capsuleB collides with sphereA
+        if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collision))
+        {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: capsule and sphere should touch"
+                << std::endl;
+        }
+    
+        // penetration points from sphereA into capsuleB
+        expectedPenetration = ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis;
+        inaccuracy = glm::length(collision._penetration - expectedPenetration);
+        if (fabs(inaccuracy) > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: bad penetration: expected = " << expectedPenetration
+                << " actual = " << collision._penetration 
+                << std::endl;
+        }
+    
+        // contactPoint is on surface of capsuleB
+        glm::vec3 endPoint;
+        capsuleB.getEndPoint(endPoint);
+        expectedContactPoint = endPoint + radiusB * yAxis;
+        inaccuracy = glm::length(collision._contactPoint - expectedContactPoint);
+        if (fabs(inaccuracy) > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: bad contactPoint: expected = " << expectedContactPoint
+                << " actual = " << collision._contactPoint 
+                << std::endl;
+        }
+    }
+    {   // sphereA hits start cap at axis
+        glm::vec3 axialOffset = - (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis;
+        sphereA.setPosition(axialOffset * yAxis);
+        
+        if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collision))
+        {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: sphere and capsule should touch"
+                << std::endl;
+        }
+    
+        // penetration points from sphereA into capsuleB
+        glm::vec3 expectedPenetration = ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis;
+        float inaccuracy = glm::length(collision._penetration - expectedPenetration);
+        if (fabs(inaccuracy) > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: bad penetration: expected = " << expectedPenetration
+                << " actual = " << collision._penetration 
+                << std::endl;
+        }
+    
+        // contactPoint is on surface of sphereA
+        glm::vec3 expectedContactPoint = sphereA.getPosition() + radiusA * yAxis;
+        inaccuracy = glm::length(collision._contactPoint - expectedContactPoint);
+        if (fabs(inaccuracy) > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: bad contactPoint: expected = " << expectedContactPoint
+                << " actual = " << collision._contactPoint 
+                << std::endl;
+        }
+
+        // capsuleB collides with sphereA
+        if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collision))
+        {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: capsule and sphere should touch"
+                << std::endl;
+        }
+    
+        // penetration points from sphereA into capsuleB
+        expectedPenetration = - ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis;
+        inaccuracy = glm::length(collision._penetration - expectedPenetration);
+        if (fabs(inaccuracy) > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: bad penetration: expected = " << expectedPenetration
+                << " actual = " << collision._penetration 
+                << std::endl;
+        }
+    
+        // contactPoint is on surface of capsuleB
+        glm::vec3 startPoint;
+        capsuleB.getStartPoint(startPoint);
+        expectedContactPoint = startPoint - radiusB * yAxis;
+        inaccuracy = glm::length(collision._contactPoint - expectedContactPoint);
+        if (fabs(inaccuracy) > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: bad contactPoint: expected = " << expectedContactPoint
+                << " actual = " << collision._contactPoint 
+                << std::endl;
+        }
+    }
+}
+
+void ShapeColliderTests::capsuleMissesCapsule() {
+    // TODO: implement this
+}
+
+void ShapeColliderTests::capsuleTouchesCapsule() {
+    // TODO: implement this
 }
-*/
 
 void ShapeColliderTests::runAllTests() {
-    ShapeColliderTests::sphereSphere();
+    sphereMissesSphere();
-//    ShapeColliderTests::test2();
+    sphereTouchesSphere();
+
+    sphereMissesCapsule();
+    sphereTouchesCapsule();
+
+    capsuleMissesCapsule();
+    capsuleTouchesCapsule();
 }

tests/physics/src/ShapeColliderTests.h

@@ -11,8 +11,14 @@
 
 namespace ShapeColliderTests {
 
-    void sphereSphere();
+    void sphereMissesSphere();
-    //void test2();
+    void sphereTouchesSphere();
+
+    void sphereMissesCapsule();
+    void sphereTouchesCapsule();
+
+    void capsuleMissesCapsule();
+    void capsuleTouchesCapsule();
 
     void runAllTests(); 
 }

tests/physics/src/main.cpp

@@ -3,13 +3,9 @@
 //  physics-tests
 //
 
-//#include <QDebug>
-
 #include "ShapeColliderTests.h"
-#include "CollisionInfoTests.h"
 
 int main(int argc, char** argv) {
-    CollisionInfoTests::runAllTests();
     ShapeColliderTests::runAllTests();
     return 0;
 }