From 82b08f1a939bfd6ad93285a5e86a1fea8d0370a2 Mon Sep 17 00:00:00 2001
From: luiscuenca <luiscuenca@outboxcode.com>
Date: Tue, 25 Sep 2018 16:21:05 -0700
Subject: [PATCH] Wait until skeleton is loaded to locate the avatar

---
 interface/src/avatar/MyAvatar.cpp | 54 ++++++++-----------------------
 interface/src/avatar/MyAvatar.h   |  2 ++
 2 files changed, 15 insertions(+), 41 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index df7ec93b6a..49ba763485 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -118,6 +118,7 @@ MyAvatar::MyAvatar(QThread* thread) :
     _goToSafe(true),
     _goToPosition(),
     _goToOrientation(),
+    _goToFeetAjustment(false),
     _prevShouldDrawHead(true),
     _audioListenerMode(FROM_HEAD),
     _hmdAtRestDetector(glm::vec3(0), glm::quat()),
@@ -498,7 +499,7 @@ void MyAvatar::update(float deltaTime) {
     setCurrentStandingHeight(computeStandingHeightMode(getControllerPoseInAvatarFrame(controller::Action::HEAD)));
     setAverageHeadRotation(computeAverageHeadRotation(getControllerPoseInAvatarFrame(controller::Action::HEAD)));
 
-   if (_drawAverageFacingEnabled) {
+    if (_drawAverageFacingEnabled) {
         auto sensorHeadPose = getControllerPoseInSensorFrame(controller::Action::HEAD);
         glm::vec3 worldHeadPos = transformPoint(getSensorToWorldMatrix(), sensorHeadPose.getTranslation());
         glm::vec3 worldFacingAverage = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacingMovingAverage.x, 0.0f, _headControllerFacingMovingAverage.y));
@@ -526,6 +527,11 @@ void MyAvatar::update(float deltaTime) {
         _physicsSafetyPending = getCollisionsEnabled();
         _characterController.recomputeFlying(); // In case we've gone to into the sky.
     }
+    if (_goToFeetAjustment && _skeletonModelLoaded) {
+        auto feetAjustment = getWorldPosition() - getWorldFeetPosition();
+        goToLocation(getWorldPosition() + feetAjustment);
+        _goToFeetAjustment = false;
+    }
     if (_physicsSafetyPending && qApp->isPhysicsEnabled() && _characterController.isEnabledAndReady()) {
         // When needed and ready, arrange to check and fix.
         _physicsSafetyPending = false;
@@ -1728,6 +1734,7 @@ void MyAvatar::setSkeletonModelURL(const QUrl& skeletonModelURL) {
 
     _headBoneSet.clear();
     _cauterizationNeedsUpdate = true;
+    _skeletonModelLoaded = false;
 
     std::shared_ptr<QMetaObject::Connection> skeletonConnection = std::make_shared<QMetaObject::Connection>();
     *skeletonConnection = QObject::connect(_skeletonModel.get(), &SkeletonModel::skeletonLoaded, [this, skeletonModelChangeCount, skeletonConnection]() {
@@ -1745,6 +1752,7 @@ void MyAvatar::setSkeletonModelURL(const QUrl& skeletonModelURL) {
            _skeletonModel->setCauterizeBoneSet(_headBoneSet);
            _fstAnimGraphOverrideUrl = _skeletonModel->getGeometry()->getAnimGraphOverrideUrl();
            initAnimGraph();
+           _skeletonModelLoaded = true;
        }
        QObject::disconnect(*skeletonConnection);
     });
@@ -2945,46 +2953,10 @@ void MyAvatar::goToLocation(const QVariant& propertiesVar) {
 }
 
 void MyAvatar::goToFeetLocation(const glm::vec3& newPosition,
-    bool hasOrientation, const glm::quat& newOrientation,
-    bool shouldFaceLocation) {
-
-    qCDebug(interfaceapp).nospace() << "MyAvatar goToFeetLocation - moving to " << newPosition.x << ", "
-        << newPosition.y << ", " << newPosition.z;
-
-    ShapeInfo shapeInfo;
-    computeShapeInfo(shapeInfo);
-    glm::vec3 halfExtents = shapeInfo.getHalfExtents();
-    glm::vec3 localFeetPos = shapeInfo.getOffset() - glm::vec3(0.0f, halfExtents.y + halfExtents.x, 0.0f);
-    glm::mat4 localFeet = createMatFromQuatAndPos(Quaternions::IDENTITY, localFeetPos);
-    
-    glm::mat4 worldFeet = createMatFromQuatAndPos(Quaternions::IDENTITY, newPosition);
-    
-    glm::mat4 avatarMat = worldFeet * glm::inverse(localFeet);
-
-    glm::vec3 adjustedPosition = extractTranslation(avatarMat);
-
-    _goToPending = true;
-    _goToPosition = adjustedPosition;
-    _goToOrientation = getWorldOrientation();
-    if (hasOrientation) {
-        qCDebug(interfaceapp).nospace() << "MyAvatar goToFeetLocation - new orientation is "
-            << newOrientation.x << ", " << newOrientation.y << ", " << newOrientation.z << ", " << newOrientation.w;
-
-        // orient the user to face the target
-        glm::quat quatOrientation = cancelOutRollAndPitch(newOrientation);
-
-        if (shouldFaceLocation) {
-            quatOrientation = newOrientation * glm::angleAxis(PI, Vectors::UP);
-
-            // move the user a couple units away
-            const float DISTANCE_TO_USER = 2.0f;
-            _goToPosition = adjustedPosition - quatOrientation * IDENTITY_FORWARD * DISTANCE_TO_USER;
-        }
-
-        _goToOrientation = quatOrientation;
-    }
-
-    emit transformChanged();
+                                bool hasOrientation, const glm::quat& newOrientation,
+                                bool shouldFaceLocation) {
+    _goToFeetAjustment = true;
+    goToLocation(newPosition, hasOrientation, newOrientation, shouldFaceLocation);
 }
 
 void MyAvatar::goToLocation(const glm::vec3& newPosition,
diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h
index 1dc0b3cd40..d7379a18c4 100644
--- a/interface/src/avatar/MyAvatar.h
+++ b/interface/src/avatar/MyAvatar.h
@@ -1732,6 +1732,7 @@ private:
     bool _goToPending { false };
     bool _physicsSafetyPending { false };
     bool _goToSafe { true };
+    bool _goToFeetAjustment { false };
     glm::vec3 _goToPosition;
     glm::quat _goToOrientation;
 
@@ -1807,6 +1808,7 @@ private:
 
     bool _haveReceivedHeightLimitsFromDomain { false };
     int _disableHandTouchCount { 0 };
+    bool _skeletonModelLoaded { false };
 
     Setting::Handle<QString> _dominantHandSetting;
     Setting::Handle<float> _headPitchSetting;