better tracking of avatar gravity setting

interface/src/avatar/MyCharacterController.cpp

@@ -61,7 +61,7 @@ void MyCharacterController::updateShapeIfNecessary() {
             if (_state == State::Hover) {
                 _rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f));
             } else {
-                _rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp);
+                _rigidBody->setGravity(_gravity * _currentUp);
             }
             // KINEMATIC_CONTROLLER_HACK
             if (_moveKinematically) {

libraries/physics/src/CharacterController.cpp

@@ -16,6 +16,7 @@
 #include <NumericalConstants.h>
 
 #include "ObjectMotionState.h"
+#include "PhysicsHelpers.h"
 #include "PhysicsLogging.h"
 
 const btVector3 LOCAL_UP_AXIS(0.0f, 1.0f, 0.0f);
@@ -112,13 +113,10 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) {
             // add to new world
             _dynamicsWorld = world;
             _pendingFlags &= ~PENDING_FLAG_JUMP;
-            // Before adding the RigidBody to the world we must save its oldGravity to the side
-            // because adding an object to the world will overwrite it with the default gravity.
-            btVector3 oldGravity = _rigidBody->getGravity();
             _dynamicsWorld->addRigidBody(_rigidBody, _collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR);
             _dynamicsWorld->addAction(this);
-            // restore gravity settings
+            // restore gravity settings because adding an object to the world overwrites its gravity setting
-            _rigidBody->setGravity(oldGravity);
+            _rigidBody->setGravity(_gravity * _currentUp);
             btCollisionShape* shape = _rigidBody->getCollisionShape();
             assert(shape && shape->getShapeType() == CAPSULE_SHAPE_PROXYTYPE);
             _ghost.setCharacterCapsule(static_cast<btCapsuleShape*>(shape)); // KINEMATIC_CONTROLLER_HACK
@@ -130,7 +128,6 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) {
         _ghost.setMaxStepHeight(0.75f * (_radius + _halfHeight)); // HACK
         _ghost.setMinWallAngle(PI / 4.0f); // HACK
         _ghost.setUpDirection(_currentUp);
-        _ghost.setGravity(DEFAULT_CHARACTER_GRAVITY);
         _ghost.setWorldTransform(_rigidBody->getWorldTransform());
     }
     if (_dynamicsWorld) {
@@ -145,7 +142,7 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) {
     }
 }
 
-bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) {
+bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld, btScalar dt) {
     _stepHeight = _minStepHeight; // clears last step obstacle
     btDispatcher* dispatcher = collisionWorld->getDispatcher();
     int numManifolds = dispatcher->getNumManifolds();
@@ -220,14 +217,12 @@ void CharacterController::preStep(btCollisionWorld* collisionWorld) {
     if (rayCallback.hasHit()) {
         _floorDistance = rayLength * rayCallback.m_closestHitFraction - _radius;
     }
-
-    _hasSupport = checkForSupport(collisionWorld);
 }
 
 const btScalar MIN_TARGET_SPEED = 0.001f;
 const btScalar MIN_TARGET_SPEED_SQUARED = MIN_TARGET_SPEED * MIN_TARGET_SPEED;
 
-void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) {
+void CharacterController::playerStep(btCollisionWorld* collisionWorld, btScalar dt) {
     btVector3 velocity = _rigidBody->getLinearVelocity() - _parentVelocity;
     if (_following) {
         _followTimeAccumulator += dt;
@@ -289,6 +284,8 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) {
         btQuaternion endRot = angularDisplacement * startRot;
         _rigidBody->setWorldTransform(btTransform(endRot, startPos));
     }
+
+    _hasSupport = checkForSupport(collisionWorld, dt);
     computeNewVelocity(dt, velocity);
 
     if (_moveKinematically) {
@@ -298,7 +295,7 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) {
         _ghost.setWorldTransform(transform);
         _ghost.setMotorVelocity(_targetVelocity);
         float overshoot = 1.0f * _radius;
-        _ghost.move(dt, overshoot);
+        _ghost.move(dt, overshoot, _gravity);
         transform.setOrigin(_ghost.getWorldTransform().getOrigin());
         _rigidBody->setWorldTransform(transform);
         _rigidBody->setLinearVelocity(_ghost.getLinearVelocity());
@@ -360,7 +357,7 @@ void CharacterController::setState(State desiredState) {
                 if (_collisionGroup == BULLET_COLLISION_GROUP_COLLISIONLESS) {
                     _rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f));
                 } else {
-                    _rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp);
+                    _rigidBody->setGravity(_gravity * _currentUp);
                 }
             }
         }
@@ -382,7 +379,7 @@ void CharacterController::setLocalBoundingBox(const glm::vec3& minCorner, const
     if (glm::abs(radius - _radius) > FLT_EPSILON || glm::abs(halfHeight - _halfHeight) > FLT_EPSILON) {
         _radius = radius;
         _halfHeight = halfHeight;
-        _minStepHeight = 0.02f; // HACK: hardcoded now but should be shape margin
+        _minStepHeight = 0.041f; // HACK: hardcoded now but should be shape margin
         _maxStepHeight = 0.75f * (_halfHeight + _radius);
 
         if (_dynamicsWorld) {
@@ -415,11 +412,8 @@ void CharacterController::handleChangedCollisionGroup() {
         _pendingFlags &= ~PENDING_FLAG_UPDATE_COLLISION_GROUP;
 
         if (_state != State::Hover && _rigidBody) {
-            if (_collisionGroup == BULLET_COLLISION_GROUP_COLLISIONLESS) {
+            _gravity = _collisionGroup == BULLET_COLLISION_GROUP_COLLISIONLESS ? 0.0f : DEFAULT_CHARACTER_GRAVITY;
-                _rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f));
+            _rigidBody->setGravity(_gravity * _currentUp);
-            } else {
-                _rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp);
-            }
         }
     }
 }
@@ -428,20 +422,14 @@ void CharacterController::updateUpAxis(const glm::quat& rotation) {
     _currentUp = quatRotate(glmToBullet(rotation), LOCAL_UP_AXIS);
     _ghost.setUpDirection(_currentUp);
     if (_state != State::Hover && _rigidBody) {
-        if (_collisionGroup == BULLET_COLLISION_GROUP_COLLISIONLESS) {
+        _rigidBody->setGravity(_gravity * _currentUp);
-            _rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f));
-        } else {
-            _rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp);
-        }
     }
 }
 
 void CharacterController::setPositionAndOrientation(
         const glm::vec3& position,
         const glm::quat& orientation) {
-    // TODO: update gravity if up has changed
     updateUpAxis(orientation);
-
     _rotation = glmToBullet(orientation);
     _position = glmToBullet(position + orientation * _shapeLocalOffset);
 }
@@ -555,10 +543,10 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel
                 // the motor pushes against step
                 btVector3 leverArm = _stepPoint;
                 motorVelocityWF = _stepNormal.cross(leverArm.cross(motorVelocityWF));
-                btScalar distortedLength = motorVelocityWF.length();
+                btScalar doubleCrossLength = motorVelocityWF.length();
-                if (distortedLength > FLT_EPSILON) {
+                if (doubleCrossLength > FLT_EPSILON) {
                     // scale the motor in the correct direction and rotate back to motor-frame
-                    motorVelocityWF *= (motorVelocity.length() / distortedLength);
+                    motorVelocityWF *= (motorVelocity.length() / doubleCrossLength);
                     motorVelocity += motorVelocityWF.rotate(axis, -angle);
                     // make vTimescale as small as possible
                     vTimescale = glm::min(vTimescale, motor.hTimescale);

libraries/physics/src/CharacterController.h

@@ -127,7 +127,7 @@ protected:
 #endif
 
     void updateUpAxis(const glm::quat& rotation);
-    bool checkForSupport(btCollisionWorld* collisionWorld);
+    bool checkForSupport(btCollisionWorld* collisionWorld, btScalar dt);
 
 protected:
     struct CharacterMotor {
@@ -176,7 +176,7 @@ protected:
     btScalar _floorDistance;
     bool _hasSupport;
 
-    btScalar _gravity;
+    btScalar _gravity { DEFAULT_CHARACTER_GRAVITY };
     btScalar _followTimeAccumulator { 0.0f };
 
     btScalar _jumpSpeed;

libraries/physics/src/CharacterGhostObject.cpp

@@ -54,11 +54,6 @@ void CharacterGhostObject::setUpDirection(const btVector3& up) {
     }
 }
 
-void CharacterGhostObject::setMotorVelocity(const btVector3& velocity) {
-    _motorVelocity = velocity;
-    _motorSpeed = _motorVelocity.length();
-}
-
 // override of btCollisionObject::setCollisionShape()
 void CharacterGhostObject::setCharacterCapsule(btCapsuleShape* capsule) {
     assert(capsule);
@@ -78,10 +73,11 @@ void CharacterGhostObject::setCollisionWorld(btCollisionWorld* world) {
     }
 }
 
-void CharacterGhostObject::move(btScalar dt, btScalar overshoot) {
+void CharacterGhostObject::move(btScalar dt, btScalar overshoot, btScalar gravity) {
     _onFloor = false;
+    _steppingUp = false;
     assert(_world && _inWorld);
-    updateVelocity(dt);
+    updateVelocity(dt, gravity);
 
     // resolve any penetrations before sweeping
     int32_t MAX_LOOPS = 4;
@@ -113,8 +109,9 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) {
         updateTraction(startPosition);
     }
 
+    btScalar speed = _linearVelocity.length();
     btVector3 forwardSweep = dt * _linearVelocity;
-    btScalar stepDistance = forwardSweep.length();
+    btScalar stepDistance = dt * speed;
     btScalar MIN_SWEEP_DISTANCE = 0.0001f;
     if (stepDistance < MIN_SWEEP_DISTANCE) {
         // not moving, no need to sweep
@@ -146,6 +143,22 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) {
         updateTraction(nextTransform.getOrigin());
         return;
     }
+    bool verticalOnly = btFabs(btFabs(_linearVelocity.dot(_upDirection)) - speed) < margin;
+    if (verticalOnly) {
+        // no need to step
+        nextTransform.setOrigin(startPosition + (result.m_closestHitFraction * stepDistance / longSweepDistance) * forwardSweep);
+        setWorldTransform(nextTransform);
+
+        if (result.m_hitNormalWorld.dot(_upDirection) > _maxWallNormalUpComponent) {
+            _floorNormal = result.m_hitNormalWorld;
+            _floorContact = result.m_hitPointWorld;
+            _steppingUp = false;
+            _onFloor = true;
+            _hovering = false;
+        }
+        updateTraction(nextTransform.getOrigin());
+        return;
+    }
 
     // check if this hit is obviously unsteppable
     btVector3 hitFromBase = result.m_hitPointWorld - (startPosition - ((_radius + _halfHeight) * _upDirection));
@@ -189,6 +202,7 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) {
         // can stand on future landing spot, so we interpolate toward it
         _floorNormal = result.m_hitNormalWorld;
         _floorContact = result.m_hitPointWorld;
+        _steppingUp = true;
         _onFloor = true;
         _hovering = false;
         nextTransform.setOrigin(startTransform.getOrigin() + result.m_closestHitFraction * downSweep);
@@ -337,11 +351,11 @@ void CharacterGhostObject::refreshOverlappingPairCache() {
     _world->getBroadphase()->setAabb(getBroadphaseHandle(), minAabb, maxAabb, _world->getDispatcher());
 }
 
-void CharacterGhostObject::updateVelocity(btScalar dt) {
+void CharacterGhostObject::updateVelocity(btScalar dt, btScalar gravity) {
     if (_hovering) {
         _linearVelocity *= 0.999f; // HACK damping
     } else {
-        _linearVelocity += (dt * _gravity) * _upDirection;
+        _linearVelocity += (dt * gravity) * _upDirection;
     }
 }
 
@@ -371,7 +385,7 @@ void CharacterGhostObject::updateTraction(const btVector3& position) {
         btVector3 pathDirection = leverArm.cross(_motorVelocity.cross(leverArm));
         btScalar pathLength = pathDirection.length();
         if (pathLength > FLT_EPSILON) {
-            _linearVelocity = (_motorSpeed / pathLength) * pathDirection;
+            _linearVelocity = (_motorVelocity.length() / pathLength) * pathDirection;
         } else {
             _linearVelocity = btVector3(0.0f, 0.0f, 0.0f);
         }

libraries/physics/src/CharacterGhostObject.h

@@ -33,18 +33,18 @@ public:
 
     void setRadiusAndHalfHeight(btScalar radius, btScalar halfHeight);
     void setUpDirection(const btVector3& up);
-    void setMotorVelocity(const btVector3& velocity);
+    void setMotorVelocity(const btVector3& velocity) { _motorVelocity = velocity; }
-    void setGravity(btScalar gravity) { _gravity = gravity; } // NOTE: we expect _gravity to be negative (in _upDirection)
     void setMinWallAngle(btScalar angle) { _maxWallNormalUpComponent = cosf(angle); }
     void setMaxStepHeight(btScalar height) { _maxStepHeight = height; }
 
+    void setLinearVelocity(const btVector3& velocity) { _linearVelocity = velocity; }
     const btVector3& getLinearVelocity() const { return _linearVelocity; }
 
     void setCharacterCapsule(btCapsuleShape* capsule);
 
     void setCollisionWorld(btCollisionWorld* world);
 
-    void move(btScalar dt, btScalar overshoot);
+    void move(btScalar dt, btScalar overshoot, btScalar gravity);
 
     bool sweepTest(const btConvexShape* shape,
             const btTransform& start,
@@ -52,6 +52,10 @@ public:
             CharacterSweepResult& result) const;
 
     bool isHovering() const { return _hovering; }
+    void setHovering(bool hovering) { _hovering = hovering; }
+
+    bool hasSupport() const { return _onFloor; }
+    bool isSteppingUp() const { return _steppingUp; }
 
 protected:
     void removeFromWorld();
@@ -63,7 +67,7 @@ protected:
 
     bool resolvePenetration(int numTries);
     void refreshOverlappingPairCache();
-    void updateVelocity(btScalar dt);
+    void updateVelocity(btScalar dt, btScalar gravity);
     void updateTraction(const btVector3& position);
     btScalar measureAvailableStepHeight() const;
     void updateHoverState(const btVector3& position);
@@ -78,8 +82,6 @@ protected:
     //btScalar _distanceToFeet { 0.0f }; // input, distance from object center to lowest point on shape
     btScalar _halfHeight { 0.0f };
     btScalar _radius { 0.0f };
-    btScalar _motorSpeed { 0.0f }; // internal, cached for speed
-    btScalar _gravity { 0.0f }; // input, amplitude of gravity along _upDirection (should be negative)
     btScalar _maxWallNormalUpComponent { 0.0f }; // input: max vertical component of wall normal
     btScalar _maxStepHeight { 0.0f }; // input, max step height the character can climb
     btCapsuleShape* _characterShape { nullptr }; // input, shape of character
@@ -89,6 +91,7 @@ protected:
     bool _inWorld { false }; // internal, was added to world
     bool _hovering { false }; // internal,
     bool _onFloor { false }; // output, is actually standing on floor
+    bool _steppingUp { false }; // output, future sweep hit a steppable ledge
     bool _hasFloor { false }; // output, has floor underneath to fall on
 };