Merge pull request #13764 from r3tk0n/17249-safe-landing

Relocate Avatar Spawn Point from the Hips to the Feet

interface/src/Application.cpp

@@ -1247,7 +1247,7 @@ Application::Application(int& argc, char** argv, QElapsedTimer& startupTimer, bo
     accountManager->setAuthURL(NetworkingConstants::METAVERSE_SERVER_URL());
 
     // use our MyAvatar position and quat for address manager path
-    addressManager->setPositionGetter([this]{ return getMyAvatar()->getWorldPosition(); });
+    addressManager->setPositionGetter([this]{ return getMyAvatar()->getWorldFeetPosition(); });
     addressManager->setOrientationGetter([this]{ return getMyAvatar()->getWorldOrientation(); });
 
     connect(addressManager.data(), &AddressManager::hostChanged, this, &Application::updateWindowTitle);

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,49 @@ 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();
+}
+
 void MyAvatar::goToLocation(const glm::vec3& newPosition,
                             bool hasOrientation, const glm::quat& newOrientation,
                             bool shouldFaceLocation) {

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

libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp

@@ -1577,6 +1577,14 @@ void Avatar::getCapsule(glm::vec3& start, glm::vec3& end, float& radius) {
     radius = halfExtents.x;
 }
 
+glm::vec3 Avatar::getWorldFeetPosition() {
+    ShapeInfo shapeInfo;
+    computeShapeInfo(shapeInfo);
+    glm::vec3 halfExtents = shapeInfo.getHalfExtents(); // x = radius, y = halfHeight
+    glm::vec3 localFeet(0.0f, shapeInfo.getOffset().y - halfExtents.y - halfExtents.x, 0.0f);
+    return getWorldOrientation() * localFeet + getWorldPosition();
+}
+
 float Avatar::computeMass() {
     float radius;
     glm::vec3 start, end;

libraries/avatars-renderer/src/avatars-renderer/Avatar.h

@@ -249,6 +249,12 @@ public:
     virtual void computeShapeInfo(ShapeInfo& shapeInfo);
     void getCapsule(glm::vec3& start, glm::vec3& end, float& radius);
     float computeMass();
+    /**jsdoc
+     * Get the position of the current avatar's feet (or rather, bottom of its collision capsule) in world coordinates.
+     * @function MyAvatar.getWorldFeetPosition
+     * @returns {Vec3} The position of the avatar's feet in world coordinates.
+    */
+    Q_INVOKABLE glm::vec3 getWorldFeetPosition();
 
     void setPositionViaScript(const glm::vec3& position) override;
     void setOrientationViaScript(const glm::quat& orientation) override;