Merge pull request #2811 from AndrewMeadows/inertia

prevent avatar from getting trapped in voxel collisions

interface/src/avatar/Avatar.cpp

@@ -634,8 +634,8 @@ bool Avatar::findParticleCollisions(const glm::vec3& particleCenter, float parti
                             penetration)) {
                     CollisionInfo* collision = collisions.getNewCollision();
                     if (collision) {
-                        collision->_type = PADDLE_HAND_COLLISION;
+                        collision->_type = COLLISION_TYPE_PADDLE_HAND;
-                        collision->_flags = jointIndex;
+                        collision->_intData = jointIndex;
                         collision->_penetration = penetration;
                         collision->_addedVelocity = palm->getVelocity();
                         collided = true;
@@ -844,11 +844,11 @@ float Avatar::getHeadHeight() const {
 }
 
 bool Avatar::collisionWouldMoveAvatar(CollisionInfo& collision) const {
-    if (!collision._data || collision._type != MODEL_COLLISION) {
+    if (!collision._data || collision._type != COLLISION_TYPE_MODEL) {
         return false;
     }
     Model* model = static_cast<Model*>(collision._data);
-    int jointIndex = collision._flags;
+    int jointIndex = collision._intData;
 
     if (model == &(_skeletonModel) && jointIndex != -1) {
         // collision response of skeleton is temporarily disabled
@@ -856,7 +856,7 @@ bool Avatar::collisionWouldMoveAvatar(CollisionInfo& collision) const {
         //return _skeletonModel.collisionHitsMoveableJoint(collision);
     }
     if (model == &(getHead()->getFaceModel())) {
-        // ATM we always handle MODEL_COLLISIONS against the face.
+        // ATM we always handle COLLISION_TYPE_MODEL against the face.
         return true;
     }
     return false;

interface/src/avatar/MyAvatar.cpp

@@ -21,6 +21,7 @@
 #include <QtCore/QTimer>
 
 #include <AccountManager.h>
+#include <GeometryUtil.h>
 #include <NodeList.h>
 #include <PacketHeaders.h>
 #include <SharedUtil.h>
@@ -63,6 +64,7 @@ MyAvatar::MyAvatar() :
     _distanceToNearestAvatar(std::numeric_limits<float>::max()),
     _wasPushing(false),
     _isPushing(false),
+    _wasStuck(false),
     _thrust(0.0f),
     _motorVelocity(0.0f),
     _motorTimescale(DEFAULT_MOTOR_TIMESCALE),
@@ -212,6 +214,8 @@ void MyAvatar::simulate(float deltaTime) {
             }
             if (_collisionGroups & COLLISION_GROUP_VOXELS) {
                 updateCollisionWithVoxels(deltaTime, radius);
+            } else {
+                _wasStuck = false;
             }
             if (_collisionGroups & COLLISION_GROUP_AVATARS) {
                 updateCollisionWithAvatars(deltaTime);
@@ -958,64 +962,47 @@ void MyAvatar::updateCollisionWithEnvironment(float deltaTime, float radius) {
 static CollisionList myCollisions(64);
 
 void MyAvatar::updateCollisionWithVoxels(float deltaTime, float radius) {
+    const float MIN_STUCK_SPEED = 100.0f;
+    float speed = glm::length(_velocity);
+    if (speed > MIN_STUCK_SPEED) {
+        // don't even bother to try to collide against voxles when moving very fast
+        return;
+    }
     myCollisions.clear();
     const CapsuleShape& boundingShape = _skeletonModel.getBoundingShape();
     if (Application::getInstance()->getVoxelTree()->findShapeCollisions(&boundingShape, myCollisions)) {
         const float VOXEL_ELASTICITY = 0.0f;
         const float VOXEL_DAMPING = 0.0f;
+        float capsuleRadius = boundingShape.getRadius();
 
-        if (glm::length2(_gravity) > EPSILON) {
+        glm::vec3 totalPenetration(0.0f);
-            if (myCollisions.size() == 1) {
+        bool isStuck = false;
-                // trivial case
-                CollisionInfo* collision = myCollisions[0];
-                applyHardCollision(collision->_penetration, VOXEL_ELASTICITY, VOXEL_DAMPING);
-                _lastFloorContactPoint = collision->_contactPoint - collision->_penetration;
-            } else {
-                // This is special collision handling for when walking on a voxel field which
-                // prevents snagging at corners and seams.
-
-                // sift through the collisions looking for one against the "floor"
-                int floorIndex = 0;
-                float distanceToFloor = 0.0f;
-                float penetrationWithFloor = 0.0f;
         for (int i = 0; i < myCollisions.size(); ++i) {
             CollisionInfo* collision = myCollisions[i];
-                    float distance = glm::dot(_gravity, collision->_contactPoint - _position);
+            float depth = glm::length(collision->_penetration);
-                    if (distance > distanceToFloor) {
+            if (depth > capsuleRadius) {
-                        distanceToFloor = distance;
+                isStuck = true;
-                        penetrationWithFloor = glm::dot(_gravity, collision->_penetration);
+                if (_wasStuck) {
-                        floorIndex = i;
+                    glm::vec3 cubeCenter = collision->_vecData;
+                    float cubeSide = collision->_floatData;
+                    float distance = glm::dot(boundingShape.getPosition() - cubeCenter, _worldUpDirection);
+                    if (distance < 0.0f) {
+                        distance = fabsf(distance) + 0.5f * cubeSide;
+                    }
+                    distance += capsuleRadius + boundingShape.getHalfHeight();
+                    totalPenetration = addPenetrations(totalPenetration, - distance * _worldUpDirection);
+                    continue;
                 }
             }
-        
+            totalPenetration = addPenetrations(totalPenetration, collision->_penetration);
-                // step through the collisions again and apply each that is not redundant
-                glm::vec3 oldPosition = _position;
-                for (int i = 0; i < myCollisions.size(); ++i) {
-                    CollisionInfo* collision = myCollisions[i];
-                    if (i == floorIndex) {
-                        applyHardCollision(collision->_penetration, VOXEL_ELASTICITY, VOXEL_DAMPING);
-                        _lastFloorContactPoint = collision->_contactPoint - collision->_penetration;
-                    } else {
-                        float distance = glm::dot(_gravity, collision->_contactPoint - oldPosition);
-                        float penetration = glm::dot(_gravity, collision->_penetration);
-                        if (fabsf(distance - distanceToFloor) > penetrationWithFloor || penetration > penetrationWithFloor) {
-                            // resolution of the deepest penetration would not resolve this one
-                            // so we apply the collision
-                            applyHardCollision(collision->_penetration, VOXEL_ELASTICITY, VOXEL_DAMPING);
-                        } 
-                    }
-                }
-            }
-        } else {
-            // no gravity -- apply all collisions
-            for (int i = 0; i < myCollisions.size(); ++i) {
-                CollisionInfo* collision = myCollisions[i];
-                applyHardCollision(collision->_penetration, VOXEL_ELASTICITY, VOXEL_DAMPING);
-            }
         }
+        applyHardCollision(totalPenetration, VOXEL_ELASTICITY, VOXEL_DAMPING);
+        _wasStuck = isStuck;
 
         const float VOXEL_COLLISION_FREQUENCY = 0.5f;
         updateCollisionSound(myCollisions[0]->_penetration, deltaTime, VOXEL_COLLISION_FREQUENCY);
+    } else {
+        _wasStuck = false;
     }
 }
 

interface/src/avatar/MyAvatar.h

@@ -125,6 +125,7 @@ private:
 
     bool _wasPushing;
     bool _isPushing;
+    bool _wasStuck;
     glm::vec3 _thrust;  // final acceleration from outside sources for the current frame
 
     glm::vec3 _motorVelocity;   // intended velocity of avatar motion

interface/src/renderer/Model.cpp

@@ -789,9 +789,9 @@ bool Model::findSphereCollisions(const glm::vec3& sphereCenter, float sphereRadi
         }
         if (ShapeCollider::collideShapes(&sphere, _jointShapes[i], collisions)) {
             CollisionInfo* collision = collisions.getLastCollision();
-            collision->_type = MODEL_COLLISION;
+            collision->_type = COLLISION_TYPE_MODEL;
             collision->_data = (void*)(this);
-            collision->_flags = i;
+            collision->_intData = i;
             collided = true;
         }
         outerContinue: ;
@@ -805,9 +805,9 @@ bool Model::findPlaneCollisions(const glm::vec4& plane, CollisionList& collision
     for (int i = 0; i < _jointShapes.size(); i++) {
         if (ShapeCollider::collideShapes(&planeShape, _jointShapes[i], collisions)) {
             CollisionInfo* collision = collisions.getLastCollision();
-            collision->_type = MODEL_COLLISION;
+            collision->_type = COLLISION_TYPE_MODEL;
             collision->_data = (void*)(this);
-            collision->_flags = i;
+            collision->_intData = i;
             collided = true;
         }
     }
@@ -1256,15 +1256,15 @@ void Model::renderBoundingCollisionShapes(float alpha) {
 }
 
 bool Model::collisionHitsMoveableJoint(CollisionInfo& collision) const {
-    if (collision._type == MODEL_COLLISION) {
+    if (collision._type == COLLISION_TYPE_MODEL) {
         // the joint is pokable by a collision if it exists and is free to move
-        const FBXJoint& joint = _geometry->getFBXGeometry().joints[collision._flags];
+        const FBXJoint& joint = _geometry->getFBXGeometry().joints[collision._intData];
         if (joint.parentIndex == -1 || _jointStates.isEmpty()) {
             return false;
         }
         // an empty freeLineage means the joint can't move
         const FBXGeometry& geometry = _geometry->getFBXGeometry();
-        int jointIndex = collision._flags;
+        int jointIndex = collision._intData;
         const QVector<int>& freeLineage = geometry.joints.at(jointIndex).freeLineage;
         return !freeLineage.isEmpty();
     }
@@ -1272,12 +1272,12 @@ bool Model::collisionHitsMoveableJoint(CollisionInfo& collision) const {
 }
 
 void Model::applyCollision(CollisionInfo& collision) {
-    if (collision._type != MODEL_COLLISION) {
+    if (collision._type != COLLISION_TYPE_MODEL) {
         return;
     }
 
     glm::vec3 jointPosition(0.f);
-    int jointIndex = collision._flags;
+    int jointIndex = collision._intData;
     if (getJointPosition(jointIndex, jointPosition)) {
         const FBXJoint& joint = _geometry->getFBXGeometry().joints[jointIndex];
         if (joint.parentIndex != -1) {

libraries/avatars/src/AvatarData.cpp

@@ -23,6 +23,7 @@
 #include <NodeList.h>
 #include <PacketHeaders.h>
 #include <SharedUtil.h>
+#include <StreamUtils.h>
 #include <UUID.h>
 #include <VoxelConstants.h>
 

libraries/avatars/src/AvatarData.h

@@ -45,7 +45,6 @@ typedef unsigned long long quint64;
 
 #include <CollisionInfo.h>
 #include <RegisteredMetaTypes.h>
-#include <StreamUtils.h>
 
 #include <Node.h>
 

libraries/particles/src/ParticleCollisionSystem.cpp

@@ -224,7 +224,7 @@ void ParticleCollisionSystem::updateCollisionWithAvatars(Particle* particle) {
                     // (doing this prevents some "collision snagging" when particle penetrates the object)
     
                     // HACK BEGIN: to allow paddle hands to "hold" particles we attenuate soft collisions against them.
-                    if (collision->_type == PADDLE_HAND_COLLISION) {
+                    if (collision->_type == COLLISION_TYPE_PADDLE_HAND) {
                         // NOTE: the physics are wrong (particles cannot roll) but it IS possible to catch a slow moving particle.
                         // TODO: make this less hacky when we have more per-collision details
                         float elasticity = ELASTICITY;

libraries/shared/src/CollisionInfo.cpp

@@ -38,19 +38,24 @@ CollisionInfo* CollisionList::getLastCollision() {
 }
 
 void CollisionList::clear() {
+    // we rely on the external context to properly set or clear the data members of a collision
+    // whenever it is used.
+    /*
     for (int i = 0; i < _size; ++i) {
         // we only clear the important stuff
         CollisionInfo& collision = _collisions[i];
-        collision._type = BASE_COLLISION;
+        collision._type = COLLISION_TYPE_UNKNOWN;
-        collision._data = NULL; // CollisionInfo does not own whatever this points to.
+        //collision._data = NULL;
-        collision._flags = 0;
+        //collision._intData = 0;
-        // we rely on the consumer to properly overwrite these fields when the collision is "created"
+        //collision._floatDAta = 0.0f;
+        //collision._vecData = glm::vec3(0.0f);
         //collision._damping;
         //collision._elasticity;
         //collision._contactPoint;
         //collision._penetration;
         //collision._addedVelocity;
     }
+    */
     _size = 0;
 }
 

libraries/shared/src/CollisionInfo.h

@@ -18,9 +18,14 @@
 #include <QVector>
 
 enum CollisionType {
-    BASE_COLLISION = 0,
+    COLLISION_TYPE_UNKNOWN = 0,
-    PADDLE_HAND_COLLISION,
+    COLLISION_TYPE_PADDLE_HAND,
-    MODEL_COLLISION,
+    COLLISION_TYPE_MODEL,
+        // _data = pointer to Model that owns joint
+        // _intData = joint index
+    COLLISION_TYPE_AACUBE,
+        // _floatData = cube side
+        // _vecData = cube center
 };
 
 const quint32 COLLISION_GROUP_ENVIRONMENT = 1U << 0;
@@ -39,7 +44,7 @@ public:
     CollisionInfo() 
         : _type(0),
         _data(NULL),
-        _flags(0),
+        _intData(0),
         _damping(0.f),
         _elasticity(1.f),
         _contactPoint(0.f), 
@@ -50,7 +55,7 @@ public:
     CollisionInfo(qint32 type)
         : _type(type),
         _data(NULL),
-        _flags(0),
+        _intData(0),
         _damping(0.f),
         _elasticity(1.f),
         _contactPoint(0.f), 
@@ -60,9 +65,13 @@ public:
 
     ~CollisionInfo() {}
 
-    qint32 _type;             // type of Collision (will determine what is supposed to be in _data and _flags)
+    int _type;          // type of Collision
-    void* _data;              // pointer to user supplied data
+
-    quint32 _flags;           // 32 bits for whatever
+    // the value of the *Data fields depend on the type 
+    void* _data;
+    int _intData;       
+    float _floatData;
+    glm::vec3 _vecData;
 
     float _damping;           // range [0,1] of friction coeficient
     float _elasticity;        // range [0,1] of energy conservation

libraries/shared/src/ShapeCollider.cpp

@@ -132,6 +132,7 @@ bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, Collis
         // penetration points from A into B
         CollisionInfo* collision = collisions.getNewCollision();
         if (collision) {
+            collision->_type = COLLISION_TYPE_UNKNOWN;
             collision->_penetration = BA * (totalRadius - distance);
             // contactPoint is on surface of A
             collision->_contactPoint = sphereA->getPosition() + sphereA->getRadius() * BA;
@@ -179,6 +180,7 @@ bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, Col
             collision->_penetration = (totalRadius - radialDistance) * radialAxis; // points from A into B
             // contactPoint is on surface of sphereA
             collision->_contactPoint = sphereA->getPosition() + sphereA->getRadius() * radialAxis;
+            collision->_type = COLLISION_TYPE_UNKNOWN;
         } else {
             // A is on B's axis, so the penetration is undefined... 
             if (absAxialDistance > capsuleB->getHalfHeight()) {
@@ -200,6 +202,7 @@ bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, Col
             collision->_penetration = (sign * (totalRadius + capsuleB->getHalfHeight() - absAxialDistance)) * capsuleAxis;
             // contactPoint is on surface of sphereA
             collision->_contactPoint = sphereA->getPosition() + (sign * sphereA->getRadius()) * capsuleAxis;
+            collision->_type = COLLISION_TYPE_UNKNOWN;
         }
         return true;
     }
@@ -215,6 +218,7 @@ bool spherePlane(const SphereShape* sphereA, const PlaneShape* planeB, Collision
         }
         collision->_penetration = penetration;
         collision->_contactPoint = sphereA->getPosition() + sphereA->getRadius() * glm::normalize(penetration);
+        collision->_type = COLLISION_TYPE_UNKNOWN;
         return true;
     }
     return false;
@@ -264,6 +268,7 @@ bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, Col
             collision->_penetration = (radialDistance - totalRadius) * radialAxis; // points from A into B
             // contactPoint is on surface of capsuleA
             collision->_contactPoint = closestApproach - capsuleA->getRadius() * radialAxis;
+            collision->_type = COLLISION_TYPE_UNKNOWN;
         } else {
             // A is on B's axis, so the penetration is undefined... 
             if (absAxialDistance > capsuleA->getHalfHeight()) {
@@ -284,6 +289,7 @@ bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, Col
                 collision->_penetration = (sign * (totalRadius + capsuleA->getHalfHeight() - absAxialDistance)) * capsuleAxis;
                 // contactPoint is on surface of sphereA
                 collision->_contactPoint = closestApproach + (sign * capsuleA->getRadius()) * capsuleAxis;
+                collision->_type = COLLISION_TYPE_UNKNOWN;
             }
         }
         return true;
@@ -355,6 +361,7 @@ bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB,
             collision->_penetration = BA * (totalRadius - distance);
             // contactPoint is on surface of A
             collision->_contactPoint = centerA + distanceA * axisA + capsuleA->getRadius() * BA;
+            collision->_type = COLLISION_TYPE_UNKNOWN;
             return true;
         }
     } else {
@@ -420,6 +427,7 @@ bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB,
             // average the internal pair, and then do the math from centerB
             collision->_contactPoint = centerB + (0.5f * (points[1] + points[2])) * axisB 
                 + (capsuleA->getRadius() - distance) * BA;
+            collision->_type = COLLISION_TYPE_UNKNOWN;
             return true;
         }
     }
@@ -439,6 +447,7 @@ bool capsulePlane(const CapsuleShape* capsuleA, const PlaneShape* planeB, Collis
         collision->_penetration = penetration;
         glm::vec3 deepestEnd = (glm::dot(start, glm::vec3(plane)) < glm::dot(end, glm::vec3(plane))) ? start : end;
         collision->_contactPoint = deepestEnd + capsuleA->getRadius() * glm::normalize(penetration);
+        collision->_type = COLLISION_TYPE_UNKNOWN;
         return true;
     }
     return false;
@@ -454,6 +463,7 @@ bool planeSphere(const PlaneShape* planeA, const SphereShape* sphereB, Collision
         collision->_penetration = -penetration;
         collision->_contactPoint = sphereB->getPosition() +
             (sphereB->getRadius() / glm::length(penetration) - 1.0f) * penetration;
+        collision->_type = COLLISION_TYPE_UNKNOWN;
         return true;
     }
     return false;
@@ -472,6 +482,7 @@ bool planeCapsule(const PlaneShape* planeA, const CapsuleShape* capsuleB, Collis
         collision->_penetration = -penetration;
         glm::vec3 deepestEnd = (glm::dot(start, glm::vec3(plane)) < glm::dot(end, glm::vec3(plane))) ? start : end;
         collision->_contactPoint = deepestEnd + (capsuleB->getRadius() / glm::length(penetration) - 1.0f) * penetration;
+        collision->_type = COLLISION_TYPE_UNKNOWN;
         return true;
     }
     return false;
@@ -653,12 +664,19 @@ bool sphereAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm::
             glm::vec3 direction;
             BA /= maxBA;
             glm::modf(BA, direction);
-            direction = glm::normalize(direction); 
+            float lengthDirection = glm::length(direction);
+            direction /= lengthDirection;
 
             // compute collision details
-            collision->_penetration = (halfCubeSide + sphereRadius - distance * glm::dot(BA, direction)) * direction;
+            collision->_type = COLLISION_TYPE_AACUBE;
+            collision->_floatData = cubeSide;
+            collision->_vecData = cubeCenter;
+            collision->_penetration = (halfCubeSide * lengthDirection + sphereRadius - maxBA * glm::dot(BA, direction)) * direction;
             collision->_contactPoint = sphereCenter + sphereRadius * direction;
         }
+        collision->_type = COLLISION_TYPE_AACUBE;
+        collision->_floatData = cubeSide;
+        collision->_vecData = cubeCenter;
         return true;
     } else if (sphereRadius + halfCubeSide > distance) {
         // NOTE: for cocentric approximation we collide sphere and cube as two spheres which means 
@@ -669,6 +687,10 @@ bool sphereAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm::
             collision->_penetration = (sphereRadius + halfCubeSide) * glm::vec3(0.0f, -1.0f, 0.0f);
             // contactPoint is on surface of A
             collision->_contactPoint = sphereCenter + collision->_penetration;
+
+            collision->_type = COLLISION_TYPE_AACUBE;
+            collision->_floatData = cubeSide;
+            collision->_vecData = cubeCenter;
             return true;
         }
     }