fix kinematic motion for ground and hover

interface/src/avatar/MyAvatar.cpp

@@ -1378,7 +1378,8 @@ void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) {
     glm::vec3 position = getPosition();
     glm::quat orientation = getOrientation();
     if (_characterController.isEnabledAndReady()) {
-        _characterController.getPositionAndOrientation(position, orientation);
+        glm::quat bogusOrientation;
+        _characterController.getPositionAndOrientation(position, bogusOrientation);
     }
     nextAttitude(position, orientation);
 

libraries/physics/src/CharacterController.cpp

@@ -130,7 +130,7 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) {
         // KINEMATIC_CONTROLLER_HACK
         _ghost.setCollisionGroupAndMask(_collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR & (~ _collisionGroup));
         _ghost.setCollisionWorld(_dynamicsWorld);
-        _ghost.setDistanceToFeet(_radius + _halfHeight);
+        _ghost.setRadiusAndHalfHeight(_radius, _halfHeight);
         _ghost.setMaxStepHeight(0.75f * (_radius + _halfHeight)); // HACK
         _ghost.setMinWallAngle(PI / 4.0f); // HACK
         _ghost.setUpDirection(_currentUp);
@@ -177,10 +177,10 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) cons
 
 void CharacterController::preStep(btCollisionWorld* collisionWorld) {
     // trace a ray straight down to see if we're standing on the ground
-    const btTransform& xform = _rigidBody->getWorldTransform();
+    const btTransform& transform = _rigidBody->getWorldTransform();
 
     // rayStart is at center of bottom sphere
-    btVector3 rayStart = xform.getOrigin() - _halfHeight * _currentUp;
+    btVector3 rayStart = transform.getOrigin() - _halfHeight * _currentUp;
 
     // rayEnd is some short distance outside bottom sphere
     const btScalar FLOOR_PROXIMITY_THRESHOLD = 0.3f * _radius;
@@ -213,7 +213,8 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) {
         _ghost.setMotorVelocity(_targetVelocity);
         float overshoot = 1.0f * _radius;
         _ghost.move(dt, overshoot);
-        _rigidBody->setWorldTransform(_ghost.getWorldTransform());
+        transform.setOrigin(_ghost.getWorldTransform().getOrigin());
+        _rigidBody->setWorldTransform(transform);
         _rigidBody->setLinearVelocity(_ghost.getLinearVelocity());
     } else {
         // Dynamicaly compute a follow velocity to move this body toward the _followDesiredBodyTransform.
@@ -400,9 +401,8 @@ void CharacterController::setPositionAndOrientation(
     // TODO: update gravity if up has changed
     updateUpAxis(orientation);
 
-    btQuaternion bodyOrientation = glmToBullet(orientation);
+    _rotation = glmToBullet(orientation);
-    btVector3 bodyPosition = glmToBullet(position + orientation * _shapeLocalOffset);
+    _position = glmToBullet(position + orientation * _shapeLocalOffset);
-    _characterBodyTransform = btTransform(bodyOrientation, bodyPosition);
 }
 
 void CharacterController::getPositionAndOrientation(glm::vec3& position, glm::quat& rotation) const {
@@ -485,10 +485,11 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel
         if (tau > 1.0f) {
             tau = 1.0f;
         }
-        velocity += (motor.velocity - velocity) * tau;
+        velocity += tau * (motor.velocity - velocity);
 
         // rotate back into world-frame
         velocity = velocity.rotate(axis, angle);
+        _targetVelocity += (tau * motor.velocity).rotate(axis, angle);
 
         // store the velocity and weight
         velocities.push_back(velocity);
@@ -584,14 +585,14 @@ void CharacterController::preSimulation() {
     if (_dynamicsWorld) {
         quint64 now = usecTimestampNow();
 
-        // slam body to where it is supposed to be
+        // slam body transform
-        _rigidBody->setWorldTransform(_characterBodyTransform);
+        _rigidBody->setWorldTransform(btTransform(btTransform(_rotation, _position)));
         btVector3 velocity = _rigidBody->getLinearVelocity();
         _preSimulationVelocity = velocity;
 
         // scan for distant floor
         // rayStart is at center of bottom sphere
-        btVector3 rayStart = _characterBodyTransform.getOrigin();
+        btVector3 rayStart = _position;
 
         // rayEnd is straight down MAX_FALL_HEIGHT
         btScalar rayLength = _radius + MAX_FALL_HEIGHT;
@@ -679,6 +680,9 @@ void CharacterController::preSimulation() {
                 }
                 break;
             }
+            if (_moveKinematically && _ghost.isHovering()) {
+                SET_STATE(State::Hover, "kinematic motion"); // HACK
+            }
         } else {
             // OUTOFBODY_HACK -- in collisionless state switch between Ground and Hover states
             if (rayHasHit) {

libraries/physics/src/CharacterController.h

@@ -153,7 +153,9 @@ protected:
     btVector3 _preSimulationVelocity;
     btVector3 _velocityChange;
     btTransform _followDesiredBodyTransform;
-    btTransform _characterBodyTransform;
+    btVector3 _position;
+    btQuaternion _rotation;
+    //btTransform _characterBodyTransform;
 
     glm::vec3 _shapeLocalOffset;
 

libraries/physics/src/CharacterGhostObject.cpp

@@ -14,6 +14,8 @@
 #include <stdint.h>
 #include <assert.h>
 
+#include <PhysicsHelpers.h>
+
 #include "CharacterGhostShape.h"
 #include "CharacterRayResult.h"
 
@@ -38,6 +40,10 @@ void CharacterGhostObject::getCollisionGroupAndMask(int16_t& group, int16_t& mas
     mask = _collisionFilterMask;
 }
 
+void CharacterGhostObject::setRadiusAndHalfHeight(btScalar radius, btScalar halfHeight) {
+    _radius = radius;
+    _halfHeight = halfHeight;
+}
 
 void CharacterGhostObject::setUpDirection(const btVector3& up) {
     btScalar length = up.length();
@@ -99,10 +105,12 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) {
 
         // TODO: figure out how to untrap character
     }
+    btTransform startTransform = getWorldTransform();
+    btVector3 startPosition = startTransform.getOrigin();
     if (_onFloor) {
-        // a floor was identified during resolvePenetration()
+        // resolvePenetration() pushed the avatar out of a floor so
-        _hovering = false;
+        // we must updateTraction() before using _linearVelocity
-        updateTraction();
+        updateTraction(startPosition);
     }
 
     btVector3 forwardSweep = dt * _linearVelocity;
@@ -110,7 +118,7 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) {
     btScalar MIN_SWEEP_DISTANCE = 0.0001f;
     if (stepDistance < MIN_SWEEP_DISTANCE) {
         // not moving, no need to sweep
-        updateHoverState(getWorldTransform());
+        updateTraction(startPosition);
         return;
     }
 
@@ -128,22 +136,19 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) {
 
     // step forward
     CharacterSweepResult result(this);
-    btTransform startTransform = getWorldTransform();
+    btTransform nextTransform = startTransform;
-    btTransform transform = startTransform;
+    nextTransform.setOrigin(startPosition + forwardSweep);
-    btTransform nextTransform = transform;
+    sweepTest(convexShape, startTransform, nextTransform, result); // forward
-    nextTransform.setOrigin(transform.getOrigin() + forwardSweep);
-    sweepTest(convexShape, transform, nextTransform, result); // forward
 
     if (!result.hasHit()) {
-        nextTransform.setOrigin(transform.getOrigin() + (stepDistance / longSweepDistance) * forwardSweep);
+        nextTransform.setOrigin(startPosition + (stepDistance / longSweepDistance) * forwardSweep);
         setWorldTransform(nextTransform);
-        updateHoverState(nextTransform);
+        updateTraction(nextTransform.getOrigin());
-        updateTraction();
         return;
     }
 
     // check if this hit is obviously unsteppable
-    btVector3 hitFromBase = result.m_hitPointWorld - (transform.getOrigin() - (_distanceToFeet * _upDirection));
+    btVector3 hitFromBase = result.m_hitPointWorld - (startPosition - ((_radius + _halfHeight) * _upDirection));
     btScalar hitHeight = hitFromBase.dot(_upDirection);
     if (hitHeight > _maxStepHeight) {
         // capsule can't step over the obstacle so move forward as much as possible before we bail
@@ -152,8 +157,8 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) {
         if (forwardDistance > stepDistance) {
             forwardTranslation *= stepDistance / forwardDistance;
         }
-        transform.setOrigin(transform.getOrigin() + forwardTranslation);
+        nextTransform.setOrigin(startPosition + forwardTranslation);
-        setWorldTransform(transform);
+        setWorldTransform(nextTransform);
         return;
     }
     // if we get here then we hit something that might be steppable
@@ -166,35 +171,37 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) {
 
     // raise by availableStepHeight before sweeping forward
     result.resetHitHistory();
-    transform.setOrigin(startTransform.getOrigin() + availableStepHeight * _upDirection);
+    startTransform.setOrigin(startPosition + availableStepHeight * _upDirection);
-    nextTransform.setOrigin(transform.getOrigin() + forwardSweep);
+    nextTransform.setOrigin(startTransform.getOrigin() + forwardSweep);
-    sweepTest(convexShape, transform, nextTransform, result);
+    sweepTest(convexShape, startTransform, nextTransform, result);
     if (result.hasHit()) {
-        transform.setOrigin(transform.getOrigin() + result.m_closestHitFraction * forwardSweep);
+        startTransform.setOrigin(startTransform.getOrigin() + result.m_closestHitFraction * forwardSweep);
     } else {
-        transform = nextTransform;
+        startTransform = nextTransform;
     }
 
     // sweep down in search of future landing spot
     result.resetHitHistory();
-    btVector3 downSweep = (dt * _linearVelocity.dot(_upDirection) - availableStepHeight) * _upDirection;
+    btVector3 downSweep = (- availableStepHeight) * _upDirection;
-    nextTransform.setOrigin(transform.getOrigin() + downSweep);
+    nextTransform.setOrigin(startTransform.getOrigin() + downSweep);
-    sweepTest(convexShape, transform, nextTransform, result);
+    sweepTest(convexShape, startTransform, nextTransform, result);
     if (result.hasHit() && result.m_hitNormalWorld.dot(_upDirection) > _maxWallNormalUpComponent) {
         // can stand on future landing spot, so we interpolate toward it
         _floorNormal = result.m_hitNormalWorld;
+        _floorContact = result.m_hitPointWorld;
         _onFloor = true;
         _hovering = false;
-        nextTransform.setOrigin(transform.getOrigin() + result.m_closestHitFraction * downSweep);
+        nextTransform.setOrigin(startTransform.getOrigin() + result.m_closestHitFraction * downSweep);
-        btVector3 totalStep = nextTransform.getOrigin() - startTransform.getOrigin();
+        btVector3 totalStep = nextTransform.getOrigin() - startPosition;
-        transform.setOrigin(startTransform.getOrigin() + (stepDistance / totalStep.length()) * totalStep);
+        nextTransform.setOrigin(startPosition + (stepDistance / totalStep.length()) * totalStep);
+        updateTraction(nextTransform.getOrigin());
     } else {
         // either there is no future landing spot, or there is but we can't stand on it
         // in any case: we go forward as much as possible
-        transform.setOrigin(startTransform.getOrigin() + forwardSweepHitFraction * (stepDistance / longSweepDistance) * forwardSweep);
+        nextTransform.setOrigin(startPosition + forwardSweepHitFraction * (stepDistance / longSweepDistance) * forwardSweep);
+        updateTraction(nextTransform.getOrigin());
     }
-    setWorldTransform(transform);
+    setWorldTransform(nextTransform);
-    updateTraction();
 }
 
 bool CharacterGhostObject::sweepTest(
@@ -297,6 +304,11 @@ bool CharacterGhostObject::resolvePenetration(int numTries) {
                     if (normalDotUp > _maxWallNormalUpComponent) {
                         mostFloorPenetration = penetrationDepth;
                         _floorNormal = normal;
+                        if (directionSign > 0.0f) {
+                            _floorContact = pt.m_positionWorldOnA;
+                        } else {
+                            _floorContact = pt.m_positionWorldOnB;
+                        }
                         _onFloor = true;
                     }
                 }
@@ -327,17 +339,36 @@ void CharacterGhostObject::refreshOverlappingPairCache() {
 
 void CharacterGhostObject::updateVelocity(btScalar dt) {
     if (_hovering) {
-        _linearVelocity *= 0.99f; // HACK damping
+        _linearVelocity *= 0.999f; // HACK damping
     } else {
         _linearVelocity += (dt * _gravity) * _upDirection;
     }
 }
 
-void CharacterGhostObject::updateTraction() {
+void CharacterGhostObject::updateHoverState(const btVector3& position) {
+    if (_onFloor) {
+        _hovering = false;
+    } else {
+        // cast a ray down looking for floor support
+        CharacterRayResult rayResult(this);
+        btScalar distanceToFeet = _radius + _halfHeight;
+        btScalar slop = 2.0f * getCollisionShape()->getMargin(); // slop to help ray start OUTSIDE the floor object
+        btVector3 startPos = position - ((distanceToFeet - slop) * _upDirection);
+        btVector3 endPos = startPos - (2.0f * distanceToFeet) * _upDirection;
+        rayTest(startPos, endPos, rayResult);
+        // we're hovering if the ray didn't hit anything or hit unstandable slope
+        _hovering = !rayResult.hasHit() || rayResult.m_hitNormalWorld.dot(_upDirection) < _maxWallNormalUpComponent;
+    }
+}
+
+void CharacterGhostObject::updateTraction(const btVector3& position) {
+    updateHoverState(position);
     if (_hovering) {
         _linearVelocity = _motorVelocity;
     } else if (_onFloor) {
-        btVector3 pathDirection = _floorNormal.cross(_motorVelocity).cross(_floorNormal);
+        // compute a velocity that swings the capsule around the _floorContact
+        btVector3 leverArm = _floorContact - position;
+        btVector3 pathDirection = leverArm.cross(_motorVelocity.cross(leverArm));
         btScalar pathLength = pathDirection.length();
         if (pathLength > FLT_EPSILON) {
             _linearVelocity = (_motorSpeed / pathLength) * pathDirection;
@@ -360,13 +391,3 @@ btScalar CharacterGhostObject::measureAvailableStepHeight() const {
     return result.m_closestHitFraction * _maxStepHeight;
 }
 
-void CharacterGhostObject::updateHoverState(const btTransform& transform) {
-    // cast a ray down looking for floor support
-    CharacterRayResult rayResult(this);
-    btVector3 startPos = transform.getOrigin() - ((_distanceToFeet - 0.1f) * _upDirection); // 0.1 HACK to make ray hit
-    btVector3 endPos = startPos - (2.0f * _distanceToFeet) * _upDirection;
-    rayTest(startPos, endPos, rayResult);
-    // we're hovering if the ray didn't hit an object we can stand on
-    _hovering = !(rayResult.hasHit() && rayResult.m_hitNormalWorld.dot(_upDirection) > _maxWallNormalUpComponent);
-}
-

libraries/physics/src/CharacterGhostObject.h

@@ -31,7 +31,7 @@ public:
     void setCollisionGroupAndMask(int16_t group, int16_t mask);
     void getCollisionGroupAndMask(int16_t& group, int16_t& mask) const;
 
-    void setDistanceToFeet(btScalar distance) { _distanceToFeet = distance; }
+    void setRadiusAndHalfHeight(btScalar radius, btScalar halfHeight);
     void setUpDirection(const btVector3& up);
     void setMotorVelocity(const btVector3& velocity);
     void setGravity(btScalar gravity) { _gravity = gravity; } // NOTE: we expect _gravity to be negative (in _upDirection)
@@ -50,6 +50,9 @@ public:
             const btTransform& start,
             const btTransform& end,
             CharacterSweepResult& result) const;
+
+    bool isHovering() const { return _hovering; }
+
 protected:
     void removeFromWorld();
     void addToWorld();
@@ -61,17 +64,20 @@ protected:
     bool resolvePenetration(int numTries);
     void refreshOverlappingPairCache();
     void updateVelocity(btScalar dt);
-    void updateTraction();
+    void updateTraction(const btVector3& position);
     btScalar measureAvailableStepHeight() const;
-    void updateHoverState(const btTransform& transform);
+    void updateHoverState(const btVector3& position);
 
 protected:
     btVector3 _upDirection { 0.0f, 1.0f, 0.0f }; // input, up in world-frame
     btVector3 _motorVelocity { 0.0f, 0.0f, 0.0f }; // input, velocity character is trying to achieve
     btVector3 _linearVelocity { 0.0f, 0.0f, 0.0f }; // internal, actual character velocity
     btVector3 _floorNormal { 0.0f, 0.0f, 0.0f }; // internal, probable floor normal
+    btVector3 _floorContact { 0.0f, 0.0f, 0.0f }; // internal, last floor contact point
     btCollisionWorld* _world { nullptr }; // input, pointer to world
-    btScalar _distanceToFeet { 0.0f }; // input, distance from object center to lowest point on shape
+    //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