From b590ff793a867169a03d6c518c7b736f7faecb1f Mon Sep 17 00:00:00 2001
From: MuteTab <najera@highfidelity.io>
Date: Mon, 6 Aug 2018 20:26:44 -0700
Subject: [PATCH] New function for changing location in domains such that the
 AVatar's leg-length is taken into account.

---
 interface/src/avatar/MyAvatar.cpp | 50 ++++++++++++++++++++++++++++++-
 interface/src/avatar/MyAvatar.h   | 14 +++++++++
 2 files changed, 63 insertions(+), 1 deletion(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 3f738ea4cb..5a5d99d6f0 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -154,7 +154,7 @@ MyAvatar::MyAvatar(QThread* thread) :
 
     // connect to AddressManager signal for location jumps
     connect(DependencyManager::get<AddressManager>().data(), &AddressManager::locationChangeRequired,
-            this, static_cast<SlotType>(&MyAvatar::goToLocation));
+            this, static_cast<SlotType>(&MyAvatar::goToFeetLocation));
 
     // handle scale constraints imposed on us by the domain-server
     auto& domainHandler = DependencyManager::get<NodeList>()->getDomainHandler();
@@ -2623,6 +2623,54 @@ 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;
+    if (hasOrientation) {
+        worldFeet = createMatFromQuatAndPos(newOrientation, newPosition);
+    } else {
+        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();
+}
+
 void MyAvatar::goToLocation(const glm::vec3& newPosition,
                             bool hasOrientation, const glm::quat& newOrientation,
                             bool shouldFaceLocation) {
diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h
index d36e43ca25..b34eb4386d 100644
--- a/interface/src/avatar/MyAvatar.h
+++ b/interface/src/avatar/MyAvatar.h
@@ -1133,6 +1133,20 @@ public slots:
      */
     float getGravity();
 
+    /**jsdoc
+     * Move the avatar to a new position and/or orientation in the domain, while taking into account Avatar leg-length.
+     * @function MyAvatar.goToFeetLocation
+     * @param {Vec3} position - The new position for the avatar, in world coordinates.
+     * @param {boolean} [hasOrientation=false] - Set to <code>true</code> to set the orientation of the avatar.
+     * @param {Quat} [orientation=Quat.IDENTITY] - The new orientation for the avatar.
+     * @param {boolean} [shouldFaceLocation=false] - Set to <code>true</code> to position the avatar a short distance away from
+     *      the new position and orientate the avatar to face the position.
+     */
+
+    void goToFeetLocation(const glm::vec3& newPosition,
+        bool hasOrientation, const glm::quat& newOrientation,
+        bool shouldFaceLocation);
+
     /**jsdoc
      * Move the avatar to a new position and/or orientation in the domain.
      * @function MyAvatar.goToLocation