swith to using shape collider instead of spheres

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"
@@ -106,8 +108,26 @@ 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;
-    if (_entities->findSpherePenetration(center, radius, penetration, (void**)&entityB, Octree::NoLock)) {
+    EntityItem* entityB = NULL;
+    
+    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/EntityTreeElement.cpp

@@ -551,25 +551,17 @@ bool EntityTreeElement::findSpherePenetration(const glm::vec3& center, float rad
 
 bool EntityTreeElement::findShapeCollisions(const Shape* shape, CollisionList& collisions) const {
     bool atLeastOneCollision = false;
-    //AACube cube = getAACube();
-    //return ShapeCollider::collideShapeWithAACubeLegacy(shape, cube.calcCenter(), cube.getScale(), collisions);
-
     QList<EntityItem*>::iterator entityItr = _entityItems->begin();
     QList<EntityItem*>::const_iterator entityEnd = _entityItems->end();
     while(entityItr != entityEnd) {
         EntityItem* entity = (*entityItr);
-        glm::vec3 entityCenter = entity->getPosition();
-        float entityRadius = entity->getRadius();
-
-        // don't collide with yourself???
-        //if (entityCenter == center && entityRadius == radius) {
-        //    return false;
-        //}
-        AACube entityAACube = entity->getMinimumAACube();
-        AACubeShape aaCube(entityAACube.getScale(), entityAACube.calcCenter());
-
-        if (ShapeCollider::collideShapes(shape, &aaCube, collisions)) {
-            atLeastOneCollision = true;
+        const Shape* otherCollisionShape = &entity->getCollisionShapeInMeters();
+        if (shape != otherCollisionShape) {
+            if (ShapeCollider::collideShapes(shape, otherCollisionShape, collisions)) {
+                CollisionInfo* lastCollision = collisions.getLastCollision();
+                lastCollision->_extraData = entity;
+                atLeastOneCollision = true;
+            }
         }
         ++entityItr;
     }

libraries/octree/src/Octree.cpp

@@ -832,7 +832,6 @@ bool findCapsulePenetrationOp(OctreeElement* element, void* extraData) {
 
 bool findShapeCollisionsOp(OctreeElement* element, void* extraData) {
     ShapeArgs* args = static_cast<ShapeArgs*>(extraData);
-
     // coarse check against bounds
     AACube cube = element->getAACube();
     cube.scale(TREE_SCALE);

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