From d2b643d389833bc7479f7cd1f278b02b4061e872 Mon Sep 17 00:00:00 2001
From: Triplelexx <mojolexx@gmail.com>
Date: Mon, 13 Apr 2015 23:57:08 +0100
Subject: [PATCH] Add targetVelocity into MyAvatar simulation

MyAvatar sets a target velocity to achieve, this is parsed in the
preSimulation controller step.
---
 interface/src/avatar/MyAvatar.cpp                  | 14 +++++++-------
 libraries/avatars/src/AvatarData.cpp               |  1 +
 libraries/avatars/src/AvatarData.h                 |  2 ++
 .../physics/src/DynamicCharacterController.cpp     |  3 ++-
 4 files changed, 12 insertions(+), 8 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 6b70577754..be438f3297 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -126,7 +126,7 @@ void MyAvatar::reset() {
     _skeletonModel.reset();
     getHead()->reset();
 
-    _velocity = glm::vec3(0.0f);
+    _targetVelocity = glm::vec3(0.0f);
     setThrust(glm::vec3(0.0f));
     //  Reset the pitch and roll components of the avatar's orientation, preserve yaw direction
     glm::vec3 eulers = safeEulerAngles(getOrientation());
@@ -1240,28 +1240,28 @@ glm::vec3 MyAvatar::applyScriptedMotor(float deltaTime, const glm::vec3& localVe
 void MyAvatar::updatePosition(float deltaTime) {
     // rotate velocity into camera frame
     glm::quat rotation = getHead()->getCameraOrientation();
-    glm::vec3 localVelocity = glm::inverse(rotation) * _velocity;
+    glm::vec3 localVelocity = glm::inverse(rotation) * _targetVelocity;
 
     bool isHovering = _characterController.isHovering();
     glm::vec3 newLocalVelocity = applyKeyboardMotor(deltaTime, localVelocity, isHovering);
     newLocalVelocity = applyScriptedMotor(deltaTime, newLocalVelocity);
 
     // rotate back into world-frame
-    _velocity = rotation * newLocalVelocity;
+    _targetVelocity = rotation * newLocalVelocity;
 
-    _velocity += _thrust * deltaTime;
+    _targetVelocity += _thrust * deltaTime;
     _thrust = glm::vec3(0.0f);
 
     // cap avatar speed
-    float speed = glm::length(_velocity);
+    float speed = glm::length(_targetVelocity);
     if (speed > MAX_AVATAR_SPEED) {
-        _velocity *= MAX_AVATAR_SPEED / speed;
+        _targetVelocity *= MAX_AVATAR_SPEED / speed;
         speed = MAX_AVATAR_SPEED;
     }
     
     if (speed > MIN_AVATAR_SPEED && !_characterController.isEnabled()) {
         // update position ourselves
-        applyPositionDelta(deltaTime * _velocity);
+        applyPositionDelta(deltaTime * _targetVelocity);
         measureMotionDerivatives(deltaTime);
     } // else physics will move avatar later
     
diff --git a/libraries/avatars/src/AvatarData.cpp b/libraries/avatars/src/AvatarData.cpp
index bc49fb3514..a8d2c209c3 100644
--- a/libraries/avatars/src/AvatarData.cpp
+++ b/libraries/avatars/src/AvatarData.cpp
@@ -60,6 +60,7 @@ AvatarData::AvatarData() :
     _owningAvatarMixer(),
     _lastUpdateTimer(),
     _velocity(0.0f),
+    _targetVelocity(0.0f),
     _localAABox(DEFAULT_LOCAL_AABOX_CORNER, DEFAULT_LOCAL_AABOX_SCALE)
 {
 }
diff --git a/libraries/avatars/src/AvatarData.h b/libraries/avatars/src/AvatarData.h
index ad90c88aaa..881b68d52f 100644
--- a/libraries/avatars/src/AvatarData.h
+++ b/libraries/avatars/src/AvatarData.h
@@ -294,6 +294,7 @@ public:
 
     void setVelocity(const glm::vec3 velocity) { _velocity = velocity; }
     Q_INVOKABLE glm::vec3 getVelocity() const { return _velocity; }
+    glm::vec3 getTargetVelocity() const { return _targetVelocity; }
 
 public slots:
     void sendAvatarDataPacket();
@@ -384,6 +385,7 @@ protected:
     void changeReferential(Referential* ref);
 
     glm::vec3 _velocity;
+    glm::vec3 _targetVelocity;
 
     AABox _localAABox;
 
diff --git a/libraries/physics/src/DynamicCharacterController.cpp b/libraries/physics/src/DynamicCharacterController.cpp
index db84bea540..15b7eb86b6 100644
--- a/libraries/physics/src/DynamicCharacterController.cpp
+++ b/libraries/physics/src/DynamicCharacterController.cpp
@@ -386,7 +386,7 @@ void DynamicCharacterController::preSimulation(btScalar timeStep) {
             setHovering(true);
         }
 
-        _walkVelocity = glmToBullet(_avatarData->getVelocity());
+        _walkVelocity = glmToBullet(_avatarData->getTargetVelocity());
 
         if (_pendingFlags & PENDING_FLAG_JUMP) {
             _pendingFlags &= ~ PENDING_FLAG_JUMP;
@@ -408,6 +408,7 @@ void DynamicCharacterController::postSimulation() {
 
         _avatarData->setOrientation(rotation);
         _avatarData->setPosition(position - rotation * _shapeLocalOffset);
+        _avatarData->setVelocity(bulletToGLM(_rigidBody->getLinearVelocity()));
     }
 }