From b561014d775003bbf94efa8c4212214e941fdf22 Mon Sep 17 00:00:00 2001
From: Ryan Huffman <ryanhuffman@gmail.com>
Date: Thu, 16 Jul 2015 14:11:36 -0700
Subject: [PATCH] Add head-lean reading back into AvatarData

---
 libraries/avatars/src/AvatarData.cpp | 24 ++++++++++++++++++++----
 1 file changed, 20 insertions(+), 4 deletions(-)

diff --git a/libraries/avatars/src/AvatarData.cpp b/libraries/avatars/src/AvatarData.cpp
index dc6f507c0d..c5e4946b9c 100644
--- a/libraries/avatars/src/AvatarData.cpp
+++ b/libraries/avatars/src/AvatarData.cpp
@@ -166,10 +166,10 @@ QByteArray AvatarData::toByteArray() {
     destinationBuffer += packFloatAngleToTwoByte(destinationBuffer, _headData->getFinalYaw());
     destinationBuffer += packFloatAngleToTwoByte(destinationBuffer, _headData->getFinalRoll());
 
-    // // Body lean
-    // destinationBuffer += packFloatAngleToTwoByte(destinationBuffer, _headData->_leanForward);
-    // destinationBuffer += packFloatAngleToTwoByte(destinationBuffer, _headData->_leanSideways);
-    // destinationBuffer += packFloatAngleToTwoByte(destinationBuffer, _headData->_torsoTwist);
+    // Body lean
+    destinationBuffer += packFloatAngleToTwoByte(destinationBuffer, _headData->_leanForward);
+    destinationBuffer += packFloatAngleToTwoByte(destinationBuffer, _headData->_leanSideways);
+    destinationBuffer += packFloatAngleToTwoByte(destinationBuffer, _headData->_torsoTwist);
 
     // Lookat Position
     memcpy(destinationBuffer, &_headData->_lookAtPosition, sizeof(_headData->_lookAtPosition));
@@ -371,6 +371,22 @@ int AvatarData::parseDataFromBuffer(const QByteArray& buffer) {
         _headData->setBaseRoll(headRoll);
     } // 6 bytes
 
+    { // Head lean (relative to pelvis)
+        float leanForward, leanSideways, torsoTwist;
+        sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t*)sourceBuffer, &leanForward);
+        sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t*)sourceBuffer, &leanSideways);
+        sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t*)sourceBuffer, &torsoTwist);
+        if (glm::isnan(leanForward) || glm::isnan(leanSideways)) {
+            if (shouldLogError(now)) {
+                qCDebug(avatars) << "Discard nan AvatarData::leanForward,leanSideways,torsoTwise; displayName = '" << _displayName << "'";
+            }
+            return maxAvailableSize;
+        }
+        _headData->_leanForward = leanForward;
+        _headData->_leanSideways = leanSideways;
+        _headData->_torsoTwist = torsoTwist;
+    } // 6 bytes
+
     { // Lookat Position
         glm::vec3 lookAt;
         memcpy(&lookAt, sourceBuffer, sizeof(lookAt));