Merge pull request #3486 from ZappoMan/moreWorkOnCollidingEntitites

Improvements to entity collisions (using shape collider)

examples/editModels.js

@@ -2449,6 +2449,7 @@ function Tooltip() {
             text += "Lifetime: " + properties.lifetime + "\n"
         }
         text += "Age: " + properties.ageAsText + "\n"
+        text += "Mass: " + properties.mass + "\n"
         text += "Script: " + properties.script + "\n"
 
 
@@ -2906,6 +2907,8 @@ function handeMenuEvent(menuItem) {
             index++;
             array.push({ label: "Linear Damping:", value: properties.damping.toFixed(decimals) });
             index++;
+            array.push({ label: "Mass:", value: properties.mass.toFixed(decimals) });
+            index++;
             array.push({ label: "Angular Pitch:", value: properties.angularVelocity.x.toFixed(decimals) });
             index++;
             array.push({ label: "Angular Yaw:", value: properties.angularVelocity.y.toFixed(decimals) });
@@ -3052,6 +3055,7 @@ Window.nonBlockingFormClosed.connect(function() {
         properties.velocity.y = array[index++].value;
         properties.velocity.z = array[index++].value;
         properties.damping = array[index++].value;
+        properties.mass = array[index++].value;
 
         properties.angularVelocity.x = array[index++].value;
         properties.angularVelocity.y = array[index++].value;

libraries/entities/src/EntityCollisionSystem.cpp

@@ -13,8 +13,10 @@
 #include <AbstractAudioInterface.h>
 #include <VoxelTree.h>
 #include <AvatarData.h>
+#include <CollisionInfo.h>
 #include <HeadData.h>
 #include <HandData.h>
+#include <SphereShape.h>
 
 #include "EntityItem.h"
 #include "EntityCollisionSystem.h"
@@ -103,11 +105,28 @@ void EntityCollisionSystem::updateCollisionWithVoxels(EntityItem* entity) {
 }
 
 void EntityCollisionSystem::updateCollisionWithEntities(EntityItem* entityA) {
-    glm::vec3 center = entityA->getPosition() * (float)(TREE_SCALE);
-    float radius = entityA->getRadius() * (float)(TREE_SCALE);
     glm::vec3 penetration;
-    EntityItem* entityB;
+    EntityItem* entityB = NULL;
-    if (_entities->findSpherePenetration(center, radius, penetration, (void**)&entityB, Octree::NoLock)) {
+    
+    const float MAX_COLLISIONS_PER_ENTITY = 32;
+    CollisionList collisions(MAX_COLLISIONS_PER_ENTITY);
+    bool shapeCollisionsAccurate = false;
+    
+    bool shapeCollisions = _entities->findShapeCollisions(&entityA->getCollisionShapeInMeters(), 
+                                            collisions, Octree::NoLock, &shapeCollisionsAccurate);
+    
+    if (shapeCollisions) {
+        for(int i = 0; i < collisions.size(); i++) {
+            CollisionInfo* collision = collisions[i];
+            penetration = collision->_penetration;
+            entityB = static_cast<EntityItem*>(collision->_extraData);
+            
+            // TODO: how to handle multiple collisions?
+            break;
+        }
+    }
+    
+    if (shapeCollisions) {
         // NOTE: 'penetration' is the depth that 'entityA' overlaps 'entityB'.  It points from A into B.
         glm::vec3 penetrationInTreeUnits = penetration / (float)(TREE_SCALE);
 

libraries/entities/src/EntityItem.cpp

@@ -70,6 +70,8 @@ void EntityItem::initFromEntityItemID(const EntityItemID& entityItemID) {
     _angularVelocity = DEFAULT_ANGULAR_VELOCITY;
     _angularDamping = DEFAULT_ANGULAR_DAMPING;
     _visible = DEFAULT_VISIBLE;
+    
+    recalculateCollisionShape();
 }
 
 EntityItem::EntityItem(const EntityItemID& entityItemID) {
@@ -490,6 +492,7 @@ int EntityItem::readEntityDataFromBuffer(const unsigned char* data, int bytesLef
 
         bytesRead += readEntitySubclassDataFromBuffer(dataAt, (bytesLeftToRead - bytesRead), args, propertyFlags, overwriteLocalData);
 
+        recalculateCollisionShape();
     }
     return bytesRead;
 }
@@ -675,7 +678,7 @@ void EntityItem::update(const quint64& updateTime) {
             velocity = NO_VELOCITY;
         }
 
-        setPosition(position);
+        setPosition(position); // this will automatically recalculate our collision shape
         setVelocity(velocity);
         
         if (wantDebug) {        
@@ -749,7 +752,7 @@ bool EntityItem::setProperties(const EntityItemProperties& properties, bool forc
         }
     }
 
-    SET_ENTITY_PROPERTY_FROM_PROPERTIES(position, setPositionInMeters);
+    SET_ENTITY_PROPERTY_FROM_PROPERTIES(position, setPositionInMeters); // this will call recalculate collision shape if needed
     SET_ENTITY_PROPERTY_FROM_PROPERTIES(dimensions, setDimensionsInMeters); // NOTE: radius is obsolete
     SET_ENTITY_PROPERTY_FROM_PROPERTIES(rotation, setRotation);
     SET_ENTITY_PROPERTY_FROM_PROPERTIES(mass, setMass);
@@ -903,3 +906,11 @@ float EntityItem::getRadius() const {
     return radius;
 }
 
+void EntityItem::recalculateCollisionShape() {
+    AACube entityAACube = getMinimumAACube();
+    entityAACube.scale(TREE_SCALE); // scale to meters
+    _collisionShape.setTranslation(entityAACube.calcCenter());
+    _collisionShape.setScale(entityAACube.getScale());
+}
+
+

libraries/entities/src/EntityItem.h

@@ -16,6 +16,7 @@
 
 #include <glm/glm.hpp>
 
+#include <AACubeShape.h>
 #include <AnimationCache.h> // for Animation, AnimationCache, and AnimationPointer classes
 #include <Octree.h> // for EncodeBitstreamParams class
 #include <OctreeElement.h> // for OctreeElement::AppendState
@@ -123,7 +124,9 @@ public:
     EntityTypes::EntityType getType() const { return _type; }
     const glm::vec3& getPosition() const { return _position; } /// get position in domain scale units (0.0 - 1.0)
     glm::vec3 getPositionInMeters() const { return _position * (float) TREE_SCALE; } /// get position in meters
-    void setPosition(const glm::vec3& value) { _position = value; } /// set position in domain scale units (0.0 - 1.0)
+    
+    /// set position in domain scale units (0.0 - 1.0)
+    void setPosition(const glm::vec3& value) { _position = value; recalculateCollisionShape(); }
     void setPositionInMeters(const glm::vec3& value) /// set position in meter units (0.0 - TREE_SCALE)
             { setPosition(glm::clamp(value / (float) TREE_SCALE, 0.0f, 1.0f)); }
 
@@ -137,14 +140,14 @@ public:
     float getLargestDimension() const { return glm::length(_dimensions); } /// get the largest possible dimension
 
     /// set dimensions in domain scale units (0.0 - 1.0) this will also reset radius appropriately
-    void setDimensions(const glm::vec3& value) { _dimensions = value; }
+    void setDimensions(const glm::vec3& value) { _dimensions = value; ; recalculateCollisionShape(); }
 
     /// set dimensions in meter units (0.0 - TREE_SCALE) this will also reset radius appropriately
     void setDimensionsInMeters(const glm::vec3& value) { setDimensions(value / (float) TREE_SCALE); }
 
     static const glm::quat DEFAULT_ROTATION;
     const glm::quat& getRotation() const { return _rotation; }
-    void setRotation(const glm::quat& rotation) { _rotation = rotation; }
+    void setRotation(const glm::quat& rotation) { _rotation = rotation; ; recalculateCollisionShape(); }
 
     static const float DEFAULT_GLOW_LEVEL;
     float getGlowLevel() const { return _glowLevel; }
@@ -207,7 +210,10 @@ public:
 
     static const glm::vec3 DEFAULT_REGISTRATION_POINT;
     const glm::vec3& getRegistrationPoint() const { return _registrationPoint; } /// registration point as ratio of entity
-    void setRegistrationPoint(const glm::vec3& value) { _registrationPoint = glm::clamp(value, 0.0f, 1.0f); } /// registration point as ratio of entity
+
+    /// registration point as ratio of entity
+    void setRegistrationPoint(const glm::vec3& value) 
+            { _registrationPoint = glm::clamp(value, 0.0f, 1.0f); recalculateCollisionShape(); }
 
     static const glm::vec3 NO_ANGULAR_VELOCITY;
     static const glm::vec3 DEFAULT_ANGULAR_VELOCITY;
@@ -229,9 +235,11 @@ public:
     float getRadius() const;
     
     void applyHardCollision(const CollisionInfo& collisionInfo);
+    virtual const Shape& getCollisionShapeInMeters() const { return _collisionShape; }
     
 protected:
     virtual void initFromEntityItemID(const EntityItemID& entityItemID); // maybe useful to allow subclasses to init
+    virtual void recalculateCollisionShape();
 
     EntityTypes::EntityType _type;
     QUuid _id;
@@ -264,6 +272,7 @@ protected:
     /// set radius in domain scale units (0.0 - 1.0) this will also reset dimensions to be equal for each axis
     void setRadius(float value); 
 
+    AACubeShape _collisionShape;
 };
 
 

libraries/entities/src/EntityItemProperties.cpp

@@ -134,6 +134,7 @@ QScriptValue EntityItemProperties::copyToScriptValue(QScriptEngine* engine) cons
     COPY_PROPERTY_TO_QSCRIPTVALUE_VEC3(velocity);
     COPY_PROPERTY_TO_QSCRIPTVALUE_VEC3(gravity);
     COPY_PROPERTY_TO_QSCRIPTVALUE(damping);
+    COPY_PROPERTY_TO_QSCRIPTVALUE(mass);
     COPY_PROPERTY_TO_QSCRIPTVALUE(lifetime);
     COPY_PROPERTY_TO_QSCRIPTVALUE_GETTER(age, getAge()); // gettable, but not settable
     COPY_PROPERTY_TO_QSCRIPTVALUE_GETTER(ageAsText, formatSecondsElapsed(getAge())); // gettable, but not settable

libraries/entities/src/EntityTreeElement.cpp

@@ -11,6 +11,9 @@
 
 #include <glm/gtx/transform.hpp>
 
+#include <AACubeShape.h>
+#include <ShapeCollider.h>
+
 #include <FBXReader.h>
 #include <GeometryUtil.h>
 
@@ -546,6 +549,25 @@ bool EntityTreeElement::findSpherePenetration(const glm::vec3& center, float rad
     return false;
 }
 
+bool EntityTreeElement::findShapeCollisions(const Shape* shape, CollisionList& collisions) const {
+    bool atLeastOneCollision = false;
+    QList<EntityItem*>::iterator entityItr = _entityItems->begin();
+    QList<EntityItem*>::const_iterator entityEnd = _entityItems->end();
+    while(entityItr != entityEnd) {
+        EntityItem* entity = (*entityItr);
+        const Shape* otherCollisionShape = &entity->getCollisionShapeInMeters();
+        if (shape != otherCollisionShape) {
+            if (ShapeCollider::collideShapes(shape, otherCollisionShape, collisions)) {
+                CollisionInfo* lastCollision = collisions.getLastCollision();
+                lastCollision->_extraData = entity;
+                atLeastOneCollision = true;
+            }
+        }
+        ++entityItr;
+    }
+    return atLeastOneCollision;
+}
+
 void EntityTreeElement::updateEntityItemID(const EntityItemID& creatorTokenEntityID, const EntityItemID& knownIDEntityID) {
     uint16_t numberOfEntities = _entityItems->size();
     for (uint16_t i = 0; i < numberOfEntities; i++) {

libraries/entities/src/EntityTreeElement.h

@@ -142,6 +142,8 @@ public:
     virtual bool findSpherePenetration(const glm::vec3& center, float radius,
                         glm::vec3& penetration, void** penetratedObject) const;
 
+    virtual bool findShapeCollisions(const Shape* shape, CollisionList& collisions) const;
+
     const QList<EntityItem*>& getEntities() const { return *_entityItems; }
     QList<EntityItem*>& getEntities() { return *_entityItems; }
     bool hasEntities() const { return _entityItems ? _entityItems->size() > 0 : false; }

libraries/entities/src/SphereEntityItem.cpp

@@ -88,4 +88,11 @@ void SphereEntityItem::appendSubclassData(OctreePacketData* packetData, EncodeBi
 
     bool successPropertyFits = true;
     APPEND_ENTITY_PROPERTY(PROP_COLOR, appendColor, getColor());
-}
+}
+
+void SphereEntityItem::recalculateCollisionShape() {
+    _sphereShape.setTranslation(getCenterInMeters());
+    glm::vec3 dimensionsInMeters = getDimensionsInMeters();
+    float largestDiameter = glm::max(dimensionsInMeters.x, dimensionsInMeters.y, dimensionsInMeters.z);
+    _sphereShape.setRadius(largestDiameter / 2.0f);
+}

libraries/entities/src/SphereEntityItem.h

@@ -12,7 +12,8 @@
 #ifndef hifi_SphereEntityItem_h
 #define hifi_SphereEntityItem_h
 
-#include "EntityItem.h" 
+#include <SphereShape.h>
+#include "EntityItem.h"
 
 class SphereEntityItem : public EntityItem {
 public:
@@ -49,9 +50,14 @@ public:
             _color[GREEN_INDEX] = value.green;
             _color[BLUE_INDEX] = value.blue;
     }
+
+    virtual const Shape& getCollisionShapeInMeters() const { return _sphereShape; }
     
 protected:
+    virtual void recalculateCollisionShape();
+
     rgbColor _color;
+    SphereShape _sphereShape;
 };
 
 #endif // hifi_SphereEntityItem_h

libraries/octree/src/Octree.cpp

@@ -817,9 +817,6 @@ bool findCapsulePenetrationOp(OctreeElement* element, void* extraData) {
     if (!box.expandedIntersectsSegment(args->start, args->end, args->radius)) {
         return false;
     }
-    if (!element->isLeaf()) {
-        return true; // recurse on children
-    }
     if (element->hasContent()) {
         glm::vec3 nodePenetration;
         if (box.findCapsulePenetration(args->start, args->end, args->radius, nodePenetration)) {
@@ -827,27 +824,29 @@ bool findCapsulePenetrationOp(OctreeElement* element, void* extraData) {
             args->found = true;
         }
     }
+    if (!element->isLeaf()) {
+        return true; // recurse on children
+    }
     return false;
 }
 
 bool findShapeCollisionsOp(OctreeElement* element, void* extraData) {
     ShapeArgs* args = static_cast<ShapeArgs*>(extraData);
-
     // coarse check against bounds
     AACube cube = element->getAACube();
     cube.scale(TREE_SCALE);
     if (!cube.expandedContains(args->shape->getTranslation(), args->shape->getBoundingRadius())) {
         return false;
     }
-    if (!element->isLeaf()) {
-        return true; // recurse on children
-    }
     if (element->hasContent()) {
-        if (ShapeCollider::collideShapeWithAACubeLegacy(args->shape, cube.calcCenter(), cube.getScale(), args->collisions)) {
+        if (element->findShapeCollisions(args->shape, args->collisions)) {
             args->found = true;
             return true;
         }
     }
+    if (!element->isLeaf()) {
+        return true; // recurse on children
+    }
     return false;
 }
 

libraries/octree/src/OctreeElement.cpp

@@ -9,6 +9,7 @@
 //  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
 //
 
+#include <assert.h>
 #include <cmath>
 #include <cstring>
 #include <stdio.h>
@@ -17,7 +18,8 @@
 
 #include <NodeList.h>
 #include <PerfStat.h>
-#include <assert.h>
+#include <AACubeShape.h>
+#include <ShapeCollider.h>
 
 #include "AACube.h"
 #include "OctalCode.h"
@@ -1369,6 +1371,11 @@ bool OctreeElement::findSpherePenetration(const glm::vec3& center, float radius,
     return _cube.findSpherePenetration(center, radius, penetration);
 }
 
+bool OctreeElement::findShapeCollisions(const Shape* shape, CollisionList& collisions) const {
+    AACube cube = getAACube();
+    cube.scale(TREE_SCALE);
+    return ShapeCollider::collideShapeWithAACubeLegacy(shape, cube.calcCenter(), cube.getScale(), collisions);
+}
 
 // TODO: consider removing this, or switching to using getOrCreateChildElementContaining(const AACube& box)...
 OctreeElement* OctreeElement::getOrCreateChildElementAt(float x, float y, float z, float s) {

libraries/octree/src/OctreeElement.h

@@ -25,6 +25,7 @@
 #include "ViewFrustum.h"
 #include "OctreeConstants.h"
 
+class CollisionList;
 class EncodeBitstreamParams;
 class Octree;
 class OctreeElement;
@@ -32,6 +33,7 @@ class OctreeElementBag;
 class OctreeElementDeleteHook;
 class OctreePacketData;
 class ReadBitstreamToTreeParams;
+class Shape;
 class VoxelSystem;
 
 const float SMALLEST_REASONABLE_OCTREE_ELEMENT_SCALE = (1.0f / TREE_SCALE) / 10000.0f; // 1/10,000th of a meter
@@ -128,6 +130,8 @@ public:
     virtual bool findSpherePenetration(const glm::vec3& center, float radius, 
                         glm::vec3& penetration, void** penetratedObject) const;
 
+    virtual bool findShapeCollisions(const Shape* shape, CollisionList& collisions) const;
+
     // Base class methods you don't need to implement
     const unsigned char* getOctalCode() const { return (_octcodePointer) ? _octalCode.pointer : &_octalCode.buffer[0]; }
     OctreeElement* getChildAtIndex(int childIndex) const;

libraries/shared/src/AACubeShape.h

@@ -12,6 +12,7 @@
 #ifndef hifi_AACubeShape_h
 #define hifi_AACubeShape_h
 
+#include <QDebug>
 #include "Shape.h"
 
 class AACubeShape : public Shape {
@@ -28,9 +29,22 @@ public:
     bool findRayIntersection(RayIntersectionInfo& intersection) const;
 
     float getVolume() const { return _scale * _scale * _scale; }
+    virtual QDebug& dumpToDebug(QDebug& debugConext) const;
 
 protected:
     float _scale;
 };
 
+inline QDebug& AACubeShape::dumpToDebug(QDebug& debugConext) const {
+    debugConext << "AACubeShape[ (" 
+            << "type: " << getType()
+            << "position: "
+            << getTranslation().x << ", " << getTranslation().y << ", " << getTranslation().z
+            << "scale: "
+            << getScale()
+            << "]";
+
+    return debugConext;
+}
+
 #endif // hifi_AACubeShape_h

libraries/shared/src/CollisionInfo.h

@@ -54,6 +54,8 @@ public:
     const Shape* _shapeA;  // pointer to shapeA in this collision
     const Shape* _shapeB;  // pointer to shapeB in this collision
 
+    void* _extraData;  // pointer to extraData for this collision, opaque to the collision info, useful for external data
+
     float _damping;           // range [0,1] of friction coeficient
     float _elasticity;        // range [0,1] of energy conservation
     glm::vec3 _contactPoint;  // world-frame point on BodyA that is deepest into BodyB

libraries/shared/src/Shape.h

@@ -14,6 +14,7 @@
 
 #include <glm/glm.hpp>
 #include <glm/gtc/quaternion.hpp>
+#include <QDebug>
 #include <QtGlobal>
 #include <QVector>
 
@@ -80,6 +81,8 @@ public:
     virtual float getVolume() const { return 1.0; }
 
     virtual void getVerletPoints(QVector<VerletPoint*>& points) {}
+    
+    virtual QDebug& dumpToDebug(QDebug& debugConext) const;
 
 protected:
     // these ctors are protected (used by derived classes only)
@@ -113,4 +116,25 @@ protected:
     float _mass;
 };
 
+inline QDebug& Shape::dumpToDebug(QDebug& debugConext) const {
+    debugConext << "Shape[ (" 
+            << "type: " << getType()
+            << "position: "
+            << getTranslation().x << ", " << getTranslation().y << ", " << getTranslation().z
+            << "radius: "
+            << getBoundingRadius()
+            << "]";
+
+    return debugConext;
+}
+
+inline QDebug operator<<(QDebug debug, const Shape& shape) {
+    return shape.dumpToDebug(debug);
+}
+
+inline QDebug operator<<(QDebug debug, const Shape* shape) {
+    return shape->dumpToDebug(debug);
+}
+
+
 #endif // hifi_Shape_h

libraries/shared/src/ShapeCollider.cpp

@@ -20,6 +20,8 @@
 #include "PlaneShape.h"
 #include "SphereShape.h"
 
+#include "StreamUtils.h"
+
 // NOTE:
 //
 // * Large ListShape's are inefficient keep the lists short.
@@ -978,7 +980,112 @@ bool aaCubeVsCapsule(const Shape* shapeA, const Shape* shapeB, CollisionList& co
     return capsuleVsAACube(shapeB, shapeA, collisions);
 }
 
+// helper function
+CollisionInfo* aaCubeVsAACubeHelper(const glm::vec3& cubeCenterA, float cubeSideA, const glm::vec3& cubeCenterB, 
+        float cubeSideB, CollisionList& collisions) {
+    // cube is A
+    // cube is B
+    // BA = B - A = from center of A to center of B 
+    float halfCubeSideA = 0.5f * cubeSideA;
+    float halfCubeSideB = 0.5f * cubeSideB;
+    glm::vec3 BA = cubeCenterB - cubeCenterA;
+
+    float distance = glm::length(BA);
+
+    if (distance > EPSILON) {
+        float maxBA = glm::max(glm::max(glm::abs(BA.x), glm::abs(BA.y)), glm::abs(BA.z));
+        if (maxBA > halfCubeSideB + halfCubeSideA) {
+            // cube misses cube entirely
+            return NULL;
+        } 
+        CollisionInfo* collision = collisions.getNewCollision();
+        if (!collision) {
+            return NULL; // no more room for collisions
+        }
+        if (maxBA > halfCubeSideB) {
+            // cube hits cube but its center is outside cube
+            // compute contact anti-pole on cube (in cube frame)
+            glm::vec3 cubeContact = glm::abs(BA);
+            if (cubeContact.x > halfCubeSideB) {
+                cubeContact.x = halfCubeSideB;
+            }
+            if (cubeContact.y > halfCubeSideB) {
+                cubeContact.y = halfCubeSideB;
+            }
+            if (cubeContact.z > halfCubeSideB) {
+                cubeContact.z = halfCubeSideB;
+            }
+            glm::vec3 signs = glm::sign(BA);
+            cubeContact.x *= signs.x;
+            cubeContact.y *= signs.y;
+            cubeContact.z *= signs.z;
+
+            // compute penetration direction
+            glm::vec3 direction = BA - cubeContact;
+            
+            float lengthDirection = glm::length(direction);
+            
+            if (lengthDirection < EPSILON) {
+                // cubeCenterA is touching cube B surface, so we can't use the difference between those two 
+                // points to compute the penetration direction.  Instead we use the unitary components of 
+                // cubeContact.
+                glm::modf(cubeContact / halfCubeSideB, direction);
+                lengthDirection = glm::length(direction);
+            } else if (lengthDirection > halfCubeSideA) {
+                collisions.deleteLastCollision();
+                return NULL;
+            }
+            direction /= lengthDirection;
+
+            // compute collision details
+            collision->_contactPoint = cubeCenterA + halfCubeSideA * direction;
+            collision->_penetration = halfCubeSideA * direction - (BA - cubeContact);
+        } else {
+            // cube center is inside cube
+            // --> push out nearest face
+            glm::vec3 direction;
+            BA /= maxBA;
+            glm::modf(BA, direction);
+            float lengthDirection = glm::length(direction);
+            direction /= lengthDirection;
+
+            // compute collision details
+            collision->_floatData = cubeSideB;
+            collision->_vecData = cubeCenterB;
+            collision->_penetration = (halfCubeSideB * lengthDirection + halfCubeSideA - maxBA * glm::dot(BA, direction)) * direction;
+            collision->_contactPoint = cubeCenterA + halfCubeSideA * direction;
+        }
+        collision->_shapeA = NULL;
+        collision->_shapeB = NULL;
+        return collision;
+    } else if (halfCubeSideA + halfCubeSideB > distance) {
+        // NOTE: for cocentric approximation we collide sphere and cube as two spheres which means 
+        // this algorithm will probably be wrong when both sphere and cube are very small (both ~EPSILON)
+        CollisionInfo* collision = collisions.getNewCollision();
+        if (collision) {
+            // the penetration and contactPoint are undefined, so we pick a penetration direction (-yAxis)
+            collision->_penetration = (halfCubeSideA + halfCubeSideB) * glm::vec3(0.0f, -1.0f, 0.0f);
+            // contactPoint is on surface of A
+            collision->_contactPoint = cubeCenterA + collision->_penetration;
+            collision->_shapeA = NULL;
+            collision->_shapeB = NULL;
+            return collision;
+        }
+    }
+    return NULL;
+}
+
 bool aaCubeVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) {
+    // BA = B - A = from center of A to center of B 
+    const AACubeShape* cubeA = static_cast<const AACubeShape*>(shapeA);
+    const AACubeShape* cubeB = static_cast<const AACubeShape*>(shapeB);
+    CollisionInfo* collision = aaCubeVsAACubeHelper( cubeA->getTranslation(), cubeA->getScale(),
+            cubeB->getTranslation(), cubeB->getScale(), collisions);
+    if (collision) {
+        collision->_shapeA = shapeA;
+        collision->_shapeB = shapeB;
+        return true;
+    }
     return false;
 }
 

libraries/shared/src/SphereShape.h

@@ -39,4 +39,26 @@ public:
     float getVolume() const { return 1.3333333333f * PI * _boundingRadius * _boundingRadius * _boundingRadius; }
 };
 
+inline QDebug operator<<(QDebug debug, const SphereShape& shape) {
+    debug << "SphereShape[ (" 
+            << "position: "
+            << shape.getTranslation().x << ", " << shape.getTranslation().y << ", " << shape.getTranslation().z
+            << "radius: "
+            << shape.getRadius()
+            << "]";
+
+    return debug;
+}
+
+inline QDebug operator<<(QDebug debug, const SphereShape* shape) {
+    debug << "SphereShape[ (" 
+            << "center: "
+            << shape->getTranslation().x << ", " << shape->getTranslation().y << ", " << shape->getTranslation().z
+            << "radius: "
+            << shape->getRadius()
+            << "]";
+
+    return debug;
+}
+
 #endif // hifi_SphereShape_h