From 1fc305ca6b6e2426754c62cc39f3a95e4be72a7c Mon Sep 17 00:00:00 2001
From: Howard Stearns <howard@highfidelity.io>
Date: Tue, 6 Oct 2015 20:51:53 -0700
Subject: [PATCH] Update HMD-derived avatar stuff during reset, update position
 under head, and remove unused vars.

---
 interface/src/avatar/MyAvatar.cpp | 24 ++++++++++++++++--------
 interface/src/avatar/MyAvatar.h   |  3 ---
 2 files changed, 16 insertions(+), 11 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 82a2231a61..68eeca429a 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -78,12 +78,10 @@ const float MyAvatar::ZOOM_DEFAULT = 1.5f;
 
 MyAvatar::MyAvatar(RigPointer rig) :
     Avatar(rig),
-    _gravity(0.0f, 0.0f, 0.0f),
     _wasPushing(false),
     _isPushing(false),
     _isBraking(false),
     _boomLength(ZOOM_DEFAULT),
-    _trapDuration(0.0f),
     _thrust(0.0f),
     _keyboardMotorVelocity(0.0f),
     _keyboardMotorTimescale(DEFAULT_KEYBOARD_MOTOR_TIMESCALE),
@@ -142,16 +140,11 @@ QByteArray MyAvatar::toByteArray(bool cullSmallChanges, bool sendAll) {
 void MyAvatar::reset() {
     _skeletonModel.reset();
     float headYaw = getHead()->getBaseYaw(); // degrees
-    getHead()->reset();
-
-    _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());
     eulers.x = 0.0f;
-    eulers.y += headYaw; // align body with head
+    eulers.y += glm::radians(headYaw); // align body with head
     eulers.z = 0.0f;
-    setOrientation(glm::quat(eulers));
 
     // This should be simpler when we have only graph animations always on.
     bool isRig = _rig->getEnableRig();
@@ -160,6 +153,21 @@ void MyAvatar::reset() {
     qApp->setRawAvatarUpdateThreading(false);
     _rig->disableHands = true;
     setEnableRigAnimations(true);
+
+    _wasPushing = _isPushing = _isBraking = _billboardValid = _goToPending = _straightingLean = false;
+    getHead()->reset();
+    _targetVelocity = glm::vec3(0.0f);
+    setThrust(glm::vec3(0.0f));
+    auto worldBodyMatrix = _sensorToWorldMatrix * _bodySensorMatrix; // Assuming these are in sync and current...
+    setPosition(extractTranslation(worldBodyMatrix)); // ...positions body under head.
+    setOrientation(glm::quat(eulers)); // Not from worldBodyMatrix, in case it is NOT in sync and current.
+    // Make sure we have current sensor data.
+    _hmdSensorMatrix = qApp->getHMDSensorPose();
+    _hmdSensorPosition = extractTranslation(_hmdSensorMatrix);
+    _hmdSensorOrientation = extractRotation(_hmdSensorMatrix);
+    _bodySensorMatrix = deriveBodyFromHMDSensor(); // Based on new HMD position and yaw (no x/z rotation), in sensor space.
+    updateSensorToWorldMatrix(); // Uses updated position/orientation and _bodySensorMatrix changes
+
     _skeletonModel.simulate(0.1f);  // non-zero
     setEnableRigAnimations(false);
     _skeletonModel.simulate(0.1f);
diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h
index 5d87737dd7..9f9027f905 100644
--- a/interface/src/avatar/MyAvatar.h
+++ b/interface/src/avatar/MyAvatar.h
@@ -282,8 +282,6 @@ private:
     // results are in sensor space
     glm::mat4 deriveBodyFromHMDSensor() const;
 
-    glm::vec3 _gravity;
-
     float _driveKeys[MAX_DRIVE_KEYS];
     bool _wasPushing;
     bool _isPushing;
@@ -291,7 +289,6 @@ private:
 
     float _boomLength;
 
-    float _trapDuration; // seconds that avatar has been trapped by collisions
     glm::vec3 _thrust;  // impulse accumulator for outside sources
 
     glm::vec3 _keyboardMotorVelocity; // target local-frame velocity of avatar (keyboard)