splitting lean input from lean perturbations

interface/src/avatar/Avatar.cpp

@@ -758,25 +758,6 @@ bool Avatar::collisionWouldMoveAvatar(CollisionInfo& collision) const {
     return false;
 }
 
-void Avatar::applyCollision(const glm::vec3& contactPoint, const glm::vec3& penetration) {
-    // compute lean angles
-    glm::vec3 leverAxis = contactPoint - getPosition();
-    float leverLength = glm::length(leverAxis);
-    if (leverLength > EPSILON) {
-        glm::quat bodyRotation = getOrientation();
-        glm::vec3 xAxis = bodyRotation * glm::vec3(1.f, 0.f, 0.f);
-        glm::vec3 zAxis = bodyRotation * glm::vec3(0.f, 0.f, 1.f);
-
-        leverAxis = leverAxis / leverLength;
-        glm::vec3 effectivePenetration = penetration - glm::dot(penetration, leverAxis) * leverAxis;
-        // we use the small-angle approximation for sine below to compute the length of 
-        // the opposite side of a narrow right triangle
-        float sideways = - glm::dot(effectivePenetration, xAxis) / leverLength;
-        float forward = glm::dot(effectivePenetration, zAxis) / leverLength;
-        getHead()->addLean(sideways, forward);
-    }
-}
-
 float Avatar::getBoundingRadius() const {
     // TODO: also use head model when computing the avatar's bounding radius
     return _skeletonModel.getBoundingRadius();

interface/src/avatar/Avatar.h

@@ -145,7 +145,7 @@ public:
     /// \return true if we expect the avatar would move as a result of the collision
     bool collisionWouldMoveAvatar(CollisionInfo& collision) const;
 
-    void applyCollision(const glm::vec3& contactPoint, const glm::vec3& penetration);
+    virtual void applyCollision(const glm::vec3& contactPoint, const glm::vec3& penetration) { }
 
     /// \return bounding radius of avatar
     virtual float getBoundingRadius() const;

interface/src/avatar/Head.cpp

@@ -39,6 +39,8 @@ Head::Head(Avatar* owningAvatar) :
     _deltaPitch(0.f),
     _deltaYaw(0.f),
     _deltaRoll(0.f),
+    _deltaLeanSideways(0.f),
+    _deltaLeanForward(0.f),
     _isCameraMoving(false),
     _faceModel(this)
 {
@@ -56,7 +58,6 @@ void Head::reset() {
 }
 
 void Head::simulate(float deltaTime, bool isMine, bool billboard) {
-    
     //  Update audio trailing average for rendering facial animations
     Faceshift* faceshift = Application::getInstance()->getFaceshift();
     Visage* visage = Application::getInstance()->getVisage();
@@ -165,6 +166,19 @@ void Head::simulate(float deltaTime, bool isMine, bool billboard) {
     _eyePosition = calculateAverageEyePosition();
 }
 
+void Head::relaxLean(float deltaTime) {
+    // restore rotation, lean to neutral positions
+    const float LEAN_RELAXATION_PERIOD = 0.25f;   // seconds
+    float relaxationFactor = 1.f - glm::min(deltaTime / LEAN_RELAXATION_PERIOD, 1.f);
+    _deltaYaw *= relaxationFactor;
+    _deltaPitch *= relaxationFactor;
+    _deltaRoll *= relaxationFactor;
+    _leanSideways *= relaxationFactor;
+    _leanForward *= relaxationFactor;
+    _deltaLeanSideways *= relaxationFactor;
+    _deltaLeanForward *= relaxationFactor;
+}
+
 void Head::render(float alpha, bool forShadowMap) {
     if (_faceModel.render(alpha, forShadowMap) && _renderLookatVectors) {
         renderLookatVectors(_leftEyePosition, _rightEyePosition, _lookAtPosition);
@@ -209,6 +223,11 @@ float Head::getFinalRoll() const {
     return glm::clamp(_baseRoll + _deltaRoll, MIN_HEAD_ROLL, MAX_HEAD_ROLL);
 }
 
+void Head::addLeanDeltas(float sideways, float forward) {
+    _deltaLeanSideways += sideways;
+    _deltaLeanForward += forward;
+}
+
 void Head::renderLookatVectors(glm::vec3 leftEyePosition, glm::vec3 rightEyePosition, glm::vec3 lookatPosition) {
 
     Application::getInstance()->getGlowEffect()->begin();

interface/src/avatar/Head.h

@@ -44,6 +44,8 @@ public:
     void setAverageLoudness(float averageLoudness) { _averageLoudness = averageLoudness; }
     void setReturnToCenter (bool returnHeadToCenter) { _returnHeadToCenter = returnHeadToCenter; }
     void setRenderLookatVectors(bool onOff) { _renderLookatVectors = onOff; }
+    void setLeanSideways(float leanSideways) { _leanSideways = leanSideways; }
+    void setLeanForward(float leanForward) { _leanForward = leanForward; }
     
     /// \return orientationBody * orientationBase+Delta
     glm::quat getFinalOrientation() const;
@@ -61,6 +63,10 @@ public:
     glm::vec3 getRightDirection() const { return getOrientation() * IDENTITY_RIGHT; }
     glm::vec3 getUpDirection() const { return getOrientation() * IDENTITY_UP; }
     glm::vec3 getFrontDirection() const { return getOrientation() * IDENTITY_FRONT; }
+    float getLeanSideways() const { return _leanSideways; }
+    float getLeanForward() const { return _leanForward; }
+    float getFinalLeanSideways() const { return _leanSideways + _deltaLeanSideways; }
+    float getFinalLeanForward() const { return _leanForward + _deltaLeanForward; }
     
     glm::quat getEyeRotation(const glm::vec3& eyePosition) const;
     
@@ -71,7 +77,7 @@ public:
     float getAverageLoudness() const { return _averageLoudness; }
     glm::vec3 calculateAverageEyePosition() { return _leftEyePosition + (_rightEyePosition - _leftEyePosition ) * ONE_HALF; }
     
-    /// Returns the point about which scaling occurs.
+    /// \return the point about which scaling occurs.
     glm::vec3 getScalePivot() const;
 
     void setDeltaPitch(float pitch) { _deltaPitch = pitch; }
@@ -86,6 +92,9 @@ public:
     virtual float getFinalPitch() const;
     virtual float getFinalYaw() const;
     virtual float getFinalRoll() const;
+
+    void relaxLean(float deltaTime);
+    void addLeanDeltas(float sideways, float forward);
     
 private:
     // disallow copies of the Head, copy of owning Avatar is disallowed too
@@ -110,11 +119,15 @@ private:
     float _rightEyeBlinkVelocity;
     float _timeWithoutTalking;
 
-    // delta angles for local head rotation
+    // delta angles for local head rotation (driven by hardware input)
     float _deltaPitch;
     float _deltaYaw;
     float _deltaRoll;
 
+    // delta lean angles for lean perturbations (driven by collisions)
+    float _deltaLeanSideways;
+    float _deltaLeanForward;
+
     bool _isCameraMoving;
     FaceModel _faceModel;
     

interface/src/avatar/MyAvatar.cpp

@@ -93,7 +93,13 @@ void MyAvatar::setMoveTarget(const glm::vec3 moveTarget) {
 }
 
 void MyAvatar::update(float deltaTime) {
+    Head* head = getHead();
+    head->relaxLean(deltaTime);
     updateFromGyros(deltaTime);
+    if (Menu::getInstance()->isOptionChecked(MenuOption::MoveWithLean)) {
+        // Faceshift drive is enabled, set the avatar drive based on the head position
+        moveWithLean();
+    }
 
     // Update head mouse from faceshift if active
     Faceshift* faceshift = Application::getInstance()->getFaceshift();
@@ -111,7 +117,6 @@ void MyAvatar::update(float deltaTime) {
         //_headMouseY = glm::clamp(_headMouseY, 0, _glWidget->height());
     }
 
-    Head* head = getHead();
     if (OculusManager::isConnected()) {
         float yaw, pitch, roll; // these angles will be in radians
         OculusManager::getEulerAngles(yaw, pitch, roll);
@@ -360,17 +365,7 @@ void MyAvatar::updateFromGyros(float deltaTime) {
                 }
             }
         }
-    } else {
-        // restore rotation, lean to neutral positions
-        const float RESTORE_PERIOD = 0.25f;   // seconds
-        float restorePercentage = glm::clamp(deltaTime/RESTORE_PERIOD, 0.f, 1.f);
-        head->setDeltaPitch(glm::mix(head->getDeltaPitch(), 0.0f, restorePercentage));
-        head->setDeltaYaw(glm::mix(head->getDeltaYaw(), 0.0f, restorePercentage));
-        head->setDeltaRoll(glm::mix(head->getDeltaRoll(), 0.0f, restorePercentage));
-        head->setLeanSideways(glm::mix(head->getLeanSideways(), 0.0f, restorePercentage));
-        head->setLeanForward(glm::mix(head->getLeanForward(), 0.0f, restorePercentage));
-        return;
-    }
+    } 
 
     // Set the rotation of the avatar's head (as seen by others, not affecting view frustum)
     // to be scaled.  Pitch is greater to emphasize nodding behavior / synchrony.
@@ -389,13 +384,11 @@ void MyAvatar::updateFromGyros(float deltaTime) {
         -MAX_LEAN, MAX_LEAN));
     head->setLeanForward(glm::clamp(glm::degrees(atanf(relativePosition.z * _leanScale / TORSO_LENGTH)),
         -MAX_LEAN, MAX_LEAN));
+}
 
-    // if Faceshift drive is enabled, set the avatar drive based on the head position
-    if (!Menu::getInstance()->isOptionChecked(MenuOption::MoveWithLean)) {
-        return;
-    }
-
+void MyAvatar::moveWithLean() {
     //  Move with Lean by applying thrust proportional to leaning
+    Head* head = getHead();
     glm::quat orientation = head->getCameraOrientation();
     glm::vec3 front = orientation * IDENTITY_FRONT;
     glm::vec3 right = orientation * IDENTITY_RIGHT;
@@ -1152,3 +1145,21 @@ void MyAvatar::goToLocationFromResponse(const QJsonObject& jsonObject) {
     }
     
 }
+
+void MyAvatar::applyCollision(const glm::vec3& contactPoint, const glm::vec3& penetration) {
+    glm::vec3 leverAxis = contactPoint - getPosition();
+    float leverLength = glm::length(leverAxis);
+    if (leverLength > EPSILON) {
+        // compute lean perturbation angles
+        glm::quat bodyRotation = getOrientation();
+        glm::vec3 xAxis = bodyRotation * glm::vec3(1.f, 0.f, 0.f);
+        glm::vec3 zAxis = bodyRotation * glm::vec3(0.f, 0.f, 1.f);
+
+        leverAxis = leverAxis / leverLength;
+        glm::vec3 effectivePenetration = penetration - glm::dot(penetration, leverAxis) * leverAxis;
+        // use the small-angle approximation for sine
+        float sideways = - glm::dot(effectivePenetration, xAxis) / leverLength;
+        float forward = glm::dot(effectivePenetration, zAxis) / leverLength;
+        getHead()->addLeanDeltas(sideways, forward);
+    }
+}

interface/src/avatar/MyAvatar.h

@@ -34,6 +34,7 @@ public:
     void update(float deltaTime);
     void simulate(float deltaTime);
     void updateFromGyros(float deltaTime);
+    void moveWithLean();
 
     void render(const glm::vec3& cameraPosition, RenderMode renderMode = NORMAL_RENDER_MODE);
     void renderBody(RenderMode renderMode);
@@ -87,6 +88,9 @@ public:
     virtual void clearJointData(int index);
     virtual void setFaceModelURL(const QUrl& faceModelURL);
     virtual void setSkeletonModelURL(const QUrl& skeletonModelURL);
+
+    void applyCollision(const glm::vec3& contactPoint, const glm::vec3& penetration);
+
 public slots:
     void goHome();
     void increaseSize();

interface/src/avatar/SkeletonModel.cpp

@@ -72,11 +72,15 @@ void SkeletonModel::getHandShapes(int jointIndex, QVector<const Shape*>& shapes)
         const FBXGeometry& geometry = _geometry->getFBXGeometry();
         for (int i = 0; i < _jointStates.size(); i++) {
             const FBXJoint& joint = geometry.joints[i];
+            int parentIndex = joint.parentIndex;
             if (i == jointIndex) {
                 // this shape is the hand
                 shapes.push_back(_shapes[i]);
+                if (parentIndex != -1) {
+                    // also add the forearm
+                    shapes.push_back(_shapes[parentIndex]);
+                }
             } else {
-                int parentIndex = joint.parentIndex;
                 while (parentIndex != -1) {
                     if (parentIndex == jointIndex) {
                         // this shape is a child of the hand
@@ -199,8 +203,8 @@ void SkeletonModel::maybeUpdateLeanRotation(const JointState& parentState, const
     glm::mat3 axes = glm::mat3_cast(_rotation);
     glm::mat3 inverse = glm::mat3(glm::inverse(parentState.transform * glm::translate(state.translation) * 
         joint.preTransform * glm::mat4_cast(joint.preRotation * joint.rotation)));
-    state.rotation = glm::angleAxis(- RADIANS_PER_DEGREE * _owningAvatar->getHead()->getLeanSideways(), 
-        glm::normalize(inverse * axes[2])) * glm::angleAxis(- RADIANS_PER_DEGREE * _owningAvatar->getHead()->getLeanForward(), 
+    state.rotation = glm::angleAxis(- RADIANS_PER_DEGREE * _owningAvatar->getHead()->getFinalLeanSideways(), 
+        glm::normalize(inverse * axes[2])) * glm::angleAxis(- RADIANS_PER_DEGREE * _owningAvatar->getHead()->getFinalLeanForward(), 
         glm::normalize(inverse * axes[0])) * joint.rotation;
 }
 

libraries/avatars/src/HeadData.cpp

@@ -61,10 +61,3 @@ void HeadData::addRoll(float roll) {
     setBaseRoll(_baseRoll + roll);
 }
 
-
-void HeadData::addLean(float sideways, float forwards) {
-    // Add lean as impulse
-    _leanSideways += sideways;
-    _leanForward  += forwards;
-}
-

libraries/avatars/src/HeadData.h

@@ -32,10 +32,6 @@ public:
     virtual ~HeadData() { };
     
     // degrees
-    float getLeanSideways() const { return _leanSideways; }
-    void setLeanSideways(float leanSideways) { _leanSideways = leanSideways; }
-    float getLeanForward() const { return _leanForward; }
-    void setLeanForward(float leanForward) { _leanForward = leanForward; }
     float getBaseYaw() const { return _baseYaw; }
     void setBaseYaw(float yaw) { _baseYaw = glm::clamp(yaw, MIN_HEAD_YAW, MAX_HEAD_YAW); }
     float getBasePitch() const { return _basePitch; }
@@ -64,7 +60,6 @@ public:
     void addYaw(float yaw);
     void addPitch(float pitch);
     void addRoll(float roll);
-    void addLean(float sideways, float forwards);
     
     const glm::vec3& getLookAtPosition() const { return _lookAtPosition; }
     void setLookAtPosition(const glm::vec3& lookAtPosition) { _lookAtPosition = lookAtPosition; }