From 9567ec11aff3c01f4c2e8f24be96248214e203c3 Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Fri, 23 Sep 2016 15:22:52 -0700
Subject: [PATCH] Prevent character from walking when pinned against collision.

This is fixed by using the pre-action velocity from CharacterController, which does not include any motors or follow velocity.
This pre-action velocity reflects the actual rigid body velocity after collision constraints are resolved.

This should prevent the character f
---
 interface/src/avatar/MyAvatar.cpp             |  4 ++++
 interface/src/avatar/MyAvatar.h               |  2 ++
 interface/src/avatar/SkeletonModel.cpp        |  2 +-
 libraries/physics/src/CharacterController.cpp | 11 +++++++++++
 libraries/physics/src/CharacterController.h   |  7 +++----
 5 files changed, 21 insertions(+), 5 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 33523fc907..d07835e44a 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -1927,6 +1927,10 @@ bool findAvatarAvatarPenetration(const glm::vec3 positionA, float radiusA, float
     return false;
 }
 
+glm::vec3 MyAvatar::getPreActionVelocity() const {
+    return _characterController.getPreActionLinearVelocity();
+}
+
 void MyAvatar::increaseSize() {
     if ((1.0f + SCALING_RATIO) * _targetScale < MAX_AVATAR_SCALE) {
         _targetScale *= (1.0f + SCALING_RATIO);
diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h
index 2ce9973ce0..d265acee93 100644
--- a/interface/src/avatar/MyAvatar.h
+++ b/interface/src/avatar/MyAvatar.h
@@ -284,6 +284,8 @@ public:
     virtual glm::quat getAbsoluteJointRotationInObjectFrame(int index) const override;
     virtual glm::vec3 getAbsoluteJointTranslationInObjectFrame(int index) const override;
 
+    glm::vec3 getPreActionVelocity() const;
+
 public slots:
     void increaseSize();
     void decreaseSize();
diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index b2bf5f64fc..47eb18ce74 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -200,7 +200,7 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
 
         Rig::CharacterControllerState ccState = convertCharacterControllerState(myAvatar->getCharacterController()->getState());
 
-        auto velocity = myAvatar->getLocalVelocity();
+        auto velocity = myAvatar->getPreActionVelocity();
         auto position = myAvatar->getLocalPosition();
         auto orientation = myAvatar->getLocalOrientation();
         _rig->computeMotionAnimationState(deltaTime, position, velocity, orientation, ccState);
diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp
index e11efa8aeb..4c62a4072f 100755
--- a/libraries/physics/src/CharacterController.cpp
+++ b/libraries/physics/src/CharacterController.cpp
@@ -195,6 +195,13 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) {
     return hasFloor;
 }
 
+void CharacterController::updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime)
+{
+    _preActionVelocity = getLinearVelocity();
+    preStep(collisionWorld);
+    playerStep(collisionWorld, deltaTime);
+}
+
 void CharacterController::preStep(btCollisionWorld* collisionWorld) {
     // trace a ray straight down to see if we're standing on the ground
     const btTransform& transform = _rigidBody->getWorldTransform();
@@ -457,6 +464,10 @@ glm::vec3 CharacterController::getLinearVelocity() const {
     return velocity;
 }
 
+glm::vec3 CharacterController::getPreActionLinearVelocity() const {
+    return _preActionVelocity;
+}
+
 glm::vec3 CharacterController::getVelocityChange() const {
     if (_rigidBody) {
         return bulletToGLM(_velocityChange);
diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h
index 9591579d63..e111c4cefe 100644
--- a/libraries/physics/src/CharacterController.h
+++ b/libraries/physics/src/CharacterController.h
@@ -60,10 +60,7 @@ public:
     virtual void warp(const btVector3& origin) override { }
     virtual void debugDraw(btIDebugDraw* debugDrawer) override { }
     virtual void setUpInterpolate(bool value) override { }
-    virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime) override {
-        preStep(collisionWorld);
-        playerStep(collisionWorld, deltaTime);
-    }
+    virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime) override;
     virtual void preStep(btCollisionWorld *collisionWorld) override;
     virtual void playerStep(btCollisionWorld *collisionWorld, btScalar dt) override;
     virtual bool canJump() const override { assert(false); return false; } // never call this
@@ -90,6 +87,7 @@ public:
     void disableFollow() { _following = false; }
 
     glm::vec3 getLinearVelocity() const;
+    glm::vec3 getPreActionLinearVelocity() const;
     glm::vec3 getVelocityChange() const;
 
     float getCapsuleRadius() const { return _radius; }
@@ -147,6 +145,7 @@ protected:
     btVector3 _targetVelocity;
     btVector3 _parentVelocity;
     btVector3 _preSimulationVelocity;
+    glm::vec3 _preActionVelocity;
     btVector3 _velocityChange;
     btTransform _followDesiredBodyTransform;
     btVector3 _position;