forgot to add the test files

tests/physics/src/VerletShapeTests.cpp

@@ -0,0 +1,768 @@
+//
+//  VerletShapeTests.cpp
+//  tests/physics/src
+//
+//  Created by Andrew Meadows on 02/21/2014.
+//  Copyright 2014 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+//#include <stdio.h>
+#include <iostream>
+#include <math.h>
+
+#include <glm/glm.hpp>
+#include <glm/gtx/quaternion.hpp>
+
+#include <CollisionInfo.h>
+#include <ShapeCollider.h>
+#include <SharedUtil.h>
+#include <VerletCapsuleShape.h>
+#include <VerletSphereShape.h>
+#include <StreamUtils.h>
+
+#include "VerletShapeTests.h"
+
+const glm::vec3 origin(0.0f);
+static const glm::vec3 xAxis(1.0f, 0.0f, 0.0f);
+static const glm::vec3 yAxis(0.0f, 1.0f, 0.0f);
+static const glm::vec3 zAxis(0.0f, 0.0f, 1.0f);
+
+void VerletShapeTests::setSpherePosition() {
+    float radius = 1.0f;
+    glm::vec3 offset(1.23f, 4.56f, 7.89f);
+    glm::vec3 point;
+    VerletSphereShape sphere(radius, &point);
+
+    point = glm::vec3(0.f);
+    float d = glm::distance(glm::vec3(0.0f), sphere.getTranslation());
+    if (d != 0.0f) {
+        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should be at origin" << std::endl;
+    }
+
+    point = offset;
+    d = glm::distance(glm::vec3(0.0f), sphere.getTranslation());
+    if (d != glm::length(offset)) {
+        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should be at offset" << std::endl;
+    }
+}
+
+void VerletShapeTests::sphereMissesSphere() {
+    // non-overlapping spheres of unequal size
+
+    float radiusA = 7.0f;
+    float radiusB = 3.0f;
+    float alpha = 1.2f;
+    float beta = 1.3f;
+    glm::vec3 offsetDirection = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f));
+    float offsetDistance = alpha * radiusA + beta * radiusB;
+
+    // create points for the sphere centers
+    glm::vec3 points[2];
+
+    // give pointers to the spheres
+    VerletSphereShape sphereA(radiusA, (points + 0));
+    VerletSphereShape sphereB(radiusB, (points + 1));
+
+    // set the positions of the spheres by slamming the points directly
+    points[0] = origin;
+    points[1] = offsetDistance * offsetDirection;
+
+    CollisionList collisions(16);
+
+    // collide A to B...
+    {
+        bool touching = ShapeCollider::collideShapes(&sphereA, &sphereB, collisions);
+        if (touching) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: sphereA and sphereB should NOT touch" << std::endl;
+        }
+    }
+
+    // collide B to A...
+    {
+        bool touching = ShapeCollider::collideShapes(&sphereB, &sphereA, collisions);
+        if (touching) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: sphereA and sphereB should NOT touch" << std::endl;
+        }
+    }
+
+    // also test shapeShape
+    {
+        bool touching = ShapeCollider::collideShapes(&sphereB, &sphereA, collisions);
+        if (touching) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: sphereA and sphereB should NOT touch" << std::endl;
+        }
+    }
+
+    if (collisions.size() > 0) {
+        std::cout << __FILE__ << ":" << __LINE__
+            << " ERROR: expected empty collision list but size is " << collisions.size()
+            << std::endl;
+    }
+}
+
+void VerletShapeTests::sphereTouchesSphere() {
+    // overlapping spheres of unequal size
+    float radiusA = 7.0f;
+    float radiusB = 3.0f;
+    float alpha = 0.2f;
+    float beta = 0.3f;
+    glm::vec3 offsetDirection = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f));
+    float offsetDistance = alpha * radiusA + beta * radiusB;
+    float expectedPenetrationDistance = (1.0f - alpha) * radiusA + (1.0f - beta) * radiusB;
+    glm::vec3 expectedPenetration = expectedPenetrationDistance * offsetDirection;
+
+    // create two points for the sphere centers
+    glm::vec3 points[2];
+
+    // give pointers to the spheres
+    VerletSphereShape sphereA(radiusA, points+0);
+    VerletSphereShape sphereB(radiusB, points+1);
+
+    // set the positions of the spheres by slamming the points directly
+    points[0] = origin;
+    points[1] = offsetDistance * offsetDirection;
+
+    CollisionList collisions(16);
+    int numCollisions = 0;
+
+    // collide A to B...
+    {
+        bool touching = ShapeCollider::collideShapes(&sphereA, &sphereB, collisions);
+        if (!touching) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: sphereA and sphereB should touch" << std::endl;
+        } else {
+            ++numCollisions;
+        }
+
+        // verify state of collisions
+        if (numCollisions != collisions.size()) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: expected collisions size of " << numCollisions << " but actual size is " << collisions.size()
+                << std::endl;
+        }
+        CollisionInfo* collision = collisions.getCollision(numCollisions - 1);
+        if (!collision) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: null collision" << 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;
+        }
+    
+        // contactPoint is on surface of sphereA
+        glm::vec3 AtoB = sphereB.getTranslation() - sphereA.getTranslation();
+        glm::vec3 expectedContactPoint = sphereA.getTranslation() + 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;
+        }
+    }
+
+    // collide B to A...
+    {
+        bool touching = ShapeCollider::collideShapes(&sphereB, &sphereA, collisions);
+        if (!touching) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: sphereA and sphereB should touch" << std::endl;
+        } else {
+            ++numCollisions;
+        }
+    
+        // penetration points from sphereA into sphereB
+        CollisionInfo* collision = collisions.getCollision(numCollisions - 1);
+        float inaccuracy = glm::length(collision->_penetration + expectedPenetration);
+        if (fabs(inaccuracy) > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: bad penetration: expected = " << expectedPenetration
+                << " actual = " << collision->_penetration;
+        }
+    
+        // contactPoint is on surface of sphereA
+        glm::vec3 BtoA = sphereA.getTranslation() - sphereB.getTranslation();
+        glm::vec3 expectedContactPoint = sphereB.getTranslation() + 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;
+        }
+    }
+}
+
+void VerletShapeTests::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;
+    
+    // create points for the sphere + capsule
+    glm::vec3 points[3];
+    for (int i = 0; i < 3; ++i) {
+        points[i] = glm::vec3(0.0f);
+    }
+
+    // give the points to the shapes
+    VerletSphereShape sphereA(radiusA, points);
+    VerletCapsuleShape capsuleB(radiusB, points+1, points+2);
+    capsuleB.setHalfHeight(halfHeightB);
+
+    // give the capsule some arbitrary transform
+    float angle = 37.8f;
+    glm::vec3 axis = glm::normalize( glm::vec3(-7.0f, 2.8f, 9.3f) );
+    glm::quat rotation = glm::angleAxis(angle, axis);
+    glm::vec3 translation(15.1f, -27.1f, -38.6f);
+    capsuleB.setRotation(rotation);
+    capsuleB.setTranslation(translation);
+
+    CollisionList collisions(16);
+
+    // walk sphereA along the local yAxis next to, but not touching, capsuleB
+    glm::vec3 localStartPosition(radialOffset, axialOffset, 0.0f);
+    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.setTranslation(rotation * localPosition + translation);
+
+        // sphereA agains capsuleB
+        if (ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions))
+        {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: sphere and capsule should NOT touch"
+                << std::endl;
+        }
+
+        // capsuleB against sphereA
+        if (ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions))
+        {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: sphere and capsule should NOT touch"
+                << std::endl;
+        }
+    }
+
+    if (collisions.size() > 0) {
+        std::cout << __FILE__ << ":" << __LINE__
+            << " ERROR: expected empty collision list but size is " << collisions.size()
+            << std::endl;
+    }
+}
+
+void VerletShapeTests::sphereTouchesCapsule() {
+    // overlapping sphere and capsule
+    float radiusA = 2.0f;
+    float radiusB = 1.0f;
+    float totalRadius = radiusA + radiusB;
+    float halfHeightB = 2.0f;
+    float alpha = 0.5f;
+    float beta = 0.5f;
+    float radialOffset = alpha * radiusA + beta * radiusB;
+    
+    // create points for the sphere + capsule
+    glm::vec3 points[3];
+    for (int i = 0; i < 3; ++i) {
+        points[i] = glm::vec3(0.0f);
+    }
+
+    // give the points to the shapes
+    VerletSphereShape sphereA(radiusA, points);
+    VerletCapsuleShape capsuleB(radiusB, points+1, points+2);
+    capsuleB.setHalfHeight(halfHeightB);
+
+    CollisionList collisions(16);
+    int numCollisions = 0;
+
+    {   // sphereA collides with capsuleB's cylindrical wall
+        sphereA.setTranslation(radialOffset * xAxis);
+
+        if (!ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions))
+        {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: sphere and capsule should touch"
+                << std::endl;
+        } else {
+            ++numCollisions;
+        }
+    
+        // penetration points from sphereA into capsuleB
+        CollisionInfo* collision = collisions.getCollision(numCollisions - 1);
+        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;
+        }
+    
+        // contactPoint is on surface of sphereA
+        glm::vec3 expectedContactPoint = sphereA.getTranslation() - radiusA * xAxis;
+        inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
+        if (fabs(inaccuracy) > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: bad contactPoint: expected = " << expectedContactPoint
+                << " actual = " << collision->_contactPoint;
+        }
+
+        // capsuleB collides with sphereA
+        if (!ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions))
+        {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: capsule and sphere should touch"
+                << std::endl;
+        } else {
+            ++numCollisions;
+        }
+    
+        // penetration points from sphereA into capsuleB
+        collision = collisions.getCollision(numCollisions - 1);
+        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;
+        }
+    
+        // contactPoint is on surface of capsuleB
+        glm::vec3 BtoA = sphereA.getTranslation() - capsuleB.getTranslation();
+        glm::vec3 closestApproach = capsuleB.getTranslation() + 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;
+        }
+    }
+    {   // sphereA hits end cap at axis
+        glm::vec3 axialOffset = (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis;
+        sphereA.setTranslation(axialOffset * yAxis);
+        
+        if (!ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions))
+        {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: sphere and capsule should touch"
+                << std::endl;
+        } else {
+            ++numCollisions;
+        }
+    
+        // penetration points from sphereA into capsuleB
+        CollisionInfo* collision = collisions.getCollision(numCollisions - 1);
+        glm::vec3 expectedPenetration = - ((1.0f - alpha) * radiusA + (1.0f - 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;
+        }
+    
+        // contactPoint is on surface of sphereA
+        glm::vec3 expectedContactPoint = sphereA.getTranslation() - radiusA * yAxis;
+        inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
+        if (fabs(inaccuracy) > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: bad contactPoint: expected = " << expectedContactPoint
+                << " actual = " << collision->_contactPoint;
+        }
+
+        // capsuleB collides with sphereA
+        if (!ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions))
+        {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: capsule and sphere should touch"
+                << std::endl;
+        } else {
+            ++numCollisions;
+        }
+    
+        // penetration points from sphereA into capsuleB
+        collision = collisions.getCollision(numCollisions - 1);
+        expectedPenetration = ((1.0f - alpha) * radiusA + (1.0f - 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;
+        }
+    
+        // 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;
+        }
+    }
+    {   // sphereA hits start cap at axis
+        glm::vec3 axialOffset = - (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis;
+        sphereA.setTranslation(axialOffset * yAxis);
+        
+        if (!ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions))
+        {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: sphere and capsule should touch"
+                << std::endl;
+        } else {
+            ++numCollisions;
+        }
+    
+        // penetration points from sphereA into capsuleB
+        CollisionInfo* collision = collisions.getCollision(numCollisions - 1);
+        glm::vec3 expectedPenetration = ((1.0f - alpha) * radiusA + (1.0f - 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;
+        }
+    
+        // contactPoint is on surface of sphereA
+        glm::vec3 expectedContactPoint = sphereA.getTranslation() + radiusA * yAxis;
+        inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
+        if (fabs(inaccuracy) > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: bad contactPoint: expected = " << expectedContactPoint
+                << " actual = " << collision->_contactPoint;
+        }
+
+        // capsuleB collides with sphereA
+        if (!ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions))
+        {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: capsule and sphere should touch"
+                << std::endl;
+        } else {
+            ++numCollisions;
+        }
+    
+        // penetration points from sphereA into capsuleB
+        collision = collisions.getCollision(numCollisions - 1);
+        expectedPenetration = - ((1.0f - alpha) * radiusA + (1.0f - 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;
+        }
+    
+        // 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;
+        }
+    }
+    if (collisions.size() != numCollisions) {
+        std::cout << __FILE__ << ":" << __LINE__
+            << " ERROR: expected " << numCollisions << " collisions but actual number is " << collisions.size()
+            << std::endl;
+    }
+}
+
+void VerletShapeTests::capsuleMissesCapsule() {
+    // non-overlapping capsules
+    float radiusA = 2.0f;
+    float halfHeightA = 3.0f;
+    float radiusB = 3.0f;
+    float halfHeightB = 4.0f;
+
+    float totalRadius = radiusA + radiusB;
+    float totalHalfLength = totalRadius + halfHeightA + halfHeightB;
+
+    // create points for the shapes
+    glm::vec3 points[4];
+    for (int i = 0; i < 4; ++i) {
+        points[i] = glm::vec3(0.0f);
+    }
+
+    // give the points to the shapes
+    VerletCapsuleShape capsuleA(radiusA, points+0, points+1);
+    VerletCapsuleShape capsuleB(radiusB, points+2, points+3);
+    capsuleA.setHalfHeight(halfHeightA);
+    capsuleA.setHalfHeight(halfHeightB);
+
+    CollisionList collisions(16);
+
+    // side by side
+    capsuleB.setTranslation((1.01f * totalRadius) * xAxis);
+    if (ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
+    {
+        std::cout << __FILE__ << ":" << __LINE__
+            << " ERROR: capsule and capsule should NOT touch"
+            << std::endl;
+    }
+    if (ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions))
+    {
+        std::cout << __FILE__ << ":" << __LINE__
+            << " ERROR: capsule and capsule should NOT touch"
+            << std::endl;
+    }
+
+    // end to end
+    capsuleB.setTranslation((1.01f * totalHalfLength) * xAxis);
+    if (ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
+    {
+        std::cout << __FILE__ << ":" << __LINE__
+            << " ERROR: capsule and capsule should NOT touch"
+            << std::endl;
+    }
+    if (ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions))
+    {
+        std::cout << __FILE__ << ":" << __LINE__
+            << " ERROR: capsule and capsule should NOT touch"
+            << std::endl;
+    }
+
+    // rotate B and move it to the side
+    glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis);
+    capsuleB.setRotation(rotation);
+    capsuleB.setTranslation((1.01f * (totalRadius + capsuleB.getHalfHeight())) * xAxis);
+    if (ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
+    {
+        std::cout << __FILE__ << ":" << __LINE__
+            << " ERROR: capsule and capsule should NOT touch"
+            << std::endl;
+    }
+    if (ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions))
+    {
+        std::cout << __FILE__ << ":" << __LINE__
+            << " ERROR: capsule and capsule should NOT touch"
+            << std::endl;
+    }
+
+    if (collisions.size() > 0) {
+        std::cout << __FILE__ << ":" << __LINE__
+            << " ERROR: expected empty collision list but size is " << collisions.size()
+            << std::endl;
+    }
+}
+
+void VerletShapeTests::capsuleTouchesCapsule() {
+    // overlapping capsules
+    float radiusA = 2.0f;
+    float halfHeightA = 3.0f;
+    float radiusB = 3.0f;
+    float halfHeightB = 4.0f;
+
+    float totalRadius = radiusA + radiusB;
+    float totalHalfLength = totalRadius + halfHeightA + halfHeightB;
+
+    // create points for the shapes
+    glm::vec3 points[4];
+    for (int i = 0; i < 4; ++i) {
+        points[i] = glm::vec3(0.0f);
+    }
+
+    // give the points to the shapes
+    VerletCapsuleShape capsuleA(radiusA, points+0, points+1);
+    VerletCapsuleShape capsuleB(radiusB, points+2, points+3);
+    capsuleA.setHalfHeight(halfHeightA);
+    capsuleB.setHalfHeight(halfHeightB);
+
+    CollisionList collisions(16);
+    int numCollisions = 0;
+
+    { // side by side
+        capsuleB.setTranslation((0.99f * totalRadius) * xAxis);
+        if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
+        {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: capsule and capsule should touch"
+                << std::endl;
+        } else {
+            ++numCollisions;
+        }
+        if (!ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions))
+        {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: capsule and capsule should touch"
+                << std::endl;
+        } else {
+            ++numCollisions;
+        }
+    }
+
+    { // end to end
+        capsuleB.setTranslation((0.99f * totalHalfLength) * yAxis);
+
+        if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
+        {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: capsule and capsule should touch"
+                << std::endl;
+        } else {
+            ++numCollisions;
+        }
+        if (!ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions))
+        {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: capsule and capsule should touch"
+                << std::endl;
+        } else {
+            ++numCollisions;
+        }
+    }
+
+    { // rotate B and move it to the side
+        glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis);
+        capsuleB.setRotation(rotation);
+        capsuleB.setTranslation((0.99f * (totalRadius + capsuleB.getHalfHeight())) * xAxis);
+
+        if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
+        {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: capsule and capsule should touch"
+                << std::endl;
+        } else {
+            ++numCollisions;
+        }
+        if (!ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions))
+        {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: capsule and capsule should touch"
+                << std::endl;
+        } else {
+            ++numCollisions;
+        }
+    }
+
+    { // again, but this time check collision details
+        float overlap = 0.1f;
+        glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis);
+        capsuleB.setRotation(rotation);
+        glm::vec3 positionB = ((totalRadius + capsuleB.getHalfHeight()) - overlap) * xAxis;
+        capsuleB.setTranslation(positionB);
+
+        // capsuleA vs capsuleB
+        if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
+        {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: capsule and capsule should touch"
+                << std::endl;
+        } else {
+            ++numCollisions;
+        }
+    
+        CollisionInfo* collision = collisions.getCollision(numCollisions - 1);
+        glm::vec3 expectedPenetration = overlap * xAxis;
+        float inaccuracy = glm::length(collision->_penetration - expectedPenetration);
+        if (fabs(inaccuracy) > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: bad penetration: expected = " << expectedPenetration
+                << " actual = " << collision->_penetration;
+        }
+    
+        glm::vec3 expectedContactPoint = capsuleA.getTranslation() + radiusA * xAxis;
+        inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
+        if (fabs(inaccuracy) > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: bad contactPoint: expected = " << expectedContactPoint
+                << " actual = " << collision->_contactPoint;
+        }
+
+        // capsuleB vs capsuleA
+        if (!ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions))
+        {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: capsule and capsule should touch"
+                << std::endl;
+        } else {
+            ++numCollisions;
+        }
+    
+        collision = collisions.getCollision(numCollisions - 1);
+        expectedPenetration = - overlap * 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;
+        }
+    
+        expectedContactPoint = capsuleB.getTranslation() - (radiusB + halfHeightB) * 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;
+        }
+    }
+
+    { // collide cylinder wall against cylinder wall
+        float overlap = 0.137f;
+        float shift = 0.317f * halfHeightA;
+        glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis);
+        capsuleB.setRotation(rotation);
+        glm::vec3 positionB = (totalRadius - overlap) * zAxis + shift * yAxis;
+        capsuleB.setTranslation(positionB);
+
+        // capsuleA vs capsuleB
+        if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
+        {
+            std::cout << __FILE__ << ":" << __LINE__
+                << " ERROR: capsule and capsule should touch"
+                << std::endl;
+        } else {
+            ++numCollisions;
+        }
+    
+        CollisionInfo* collision = collisions.getCollision(numCollisions - 1);
+        glm::vec3 expectedPenetration = overlap * zAxis;
+        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;
+        }
+    
+        glm::vec3 expectedContactPoint = capsuleA.getTranslation() + radiusA * zAxis + shift * 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 VerletShapeTests::runAllTests() {
+    setSpherePosition();
+    sphereMissesSphere();
+    sphereTouchesSphere();
+
+    sphereMissesCapsule();
+    sphereTouchesCapsule();
+
+    capsuleMissesCapsule();
+    capsuleTouchesCapsule();
+}

tests/physics/src/VerletShapeTests.h

@@ -0,0 +1,30 @@
+//
+//  VerletShapeTests.h
+//  tests/physics/src
+//
+//  Created by Andrew Meadows on 2014.06.18
+//  Copyright 2014 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#ifndef hifi_VerletShapeTests_h
+#define hifi_VerletShapeTests_h
+
+namespace VerletShapeTests {
+    void setSpherePosition();
+
+    void sphereMissesSphere();
+    void sphereTouchesSphere();
+
+    void sphereMissesCapsule();
+    void sphereTouchesCapsule();
+
+    void capsuleMissesCapsule();
+    void capsuleTouchesCapsule();
+
+    void runAllTests(); 
+}
+
+#endif // hifi_VerletShapeTests_h