Merge pull request #11760 from druiz17/far-grab-parent

Far grab rolls up parent tree when grabbing and add offset to LaserPointer::lockEndUUID

interface/src/raypick/LaserPointer.cpp

@@ -25,7 +25,6 @@ LaserPointer::LaserPointer(const QVariant& rayProps, const RenderStateMap& rende
     _distanceScaleEnd(distanceScaleEnd),
     _rayPickUID(DependencyManager::get<RayPickScriptingInterface>()->createRayPick(rayProps))
 {
-    
 
     for (auto& state : _renderStates) {
         if (!enabled || state.first != _currentRenderState) {
@@ -119,23 +118,25 @@ void LaserPointer::updateRenderState(const RenderState& renderState, const Inter
         qApp->getOverlays().editOverlay(renderState.getStartID(), startProps);
     }
     glm::vec3 endVec;
-    if (((defaultState || !_lockEnd) && _objectLockEnd.first.isNull()) || type == IntersectionType::HUD) {
+    if (((defaultState || !_lockEnd) && _lockEndObject.id.isNull()) || type == IntersectionType::HUD) {
         endVec = pickRay.origin + pickRay.direction * distance;
     } else {
-        if (!_objectLockEnd.first.isNull()) {
+        if (!_lockEndObject.id.isNull()) {
             glm::vec3 pos;
             glm::quat rot;
             glm::vec3 dim;
             glm::vec3 registrationPoint;
-            if (_objectLockEnd.second) {
+            if (_lockEndObject.isOverlay) {
-                pos = vec3FromVariant(qApp->getOverlays().getProperty(_objectLockEnd.first, "position").value);
+                pos = vec3FromVariant(qApp->getOverlays().getProperty(_lockEndObject.id, "position").value);
-                rot = quatFromVariant(qApp->getOverlays().getProperty(_objectLockEnd.first, "rotation").value);
+                rot = quatFromVariant(qApp->getOverlays().getProperty(_lockEndObject.id, "rotation").value);
-                dim = vec3FromVariant(qApp->getOverlays().getProperty(_objectLockEnd.first, "dimensions").value);
+                dim = vec3FromVariant(qApp->getOverlays().getProperty(_lockEndObject.id, "dimensions").value);
                 registrationPoint = glm::vec3(0.5f);
             } else {
-                EntityItemProperties props = DependencyManager::get<EntityScriptingInterface>()->getEntityProperties(_objectLockEnd.first);
+                EntityItemProperties props = DependencyManager::get<EntityScriptingInterface>()->getEntityProperties(_lockEndObject.id);
-                pos = props.getPosition();
+                glm::mat4 entityMat = createMatFromQuatAndPos(props.getRotation(), props.getPosition());
-                rot = props.getRotation();
+                glm::mat4 finalPosAndRotMat = entityMat * _lockEndObject.offsetMat;
+                pos = extractTranslation(finalPosAndRotMat);
+                rot = glmExtractRotation(finalPosAndRotMat);
                 dim = props.getDimensions();
                 registrationPoint = props.getRegistrationPoint();
             }
@@ -209,7 +210,7 @@ void LaserPointer::update() {
     withReadLock([&] {
         RayPickResult prevRayPickResult = qApp->getRayPickManager().getPrevRayPickResult(_rayPickUID);
         if (_renderingEnabled && !_currentRenderState.empty() && _renderStates.find(_currentRenderState) != _renderStates.end() &&
-            (prevRayPickResult.type != IntersectionType::NONE || _laserLength > 0.0f || !_objectLockEnd.first.isNull())) {
+            (prevRayPickResult.type != IntersectionType::NONE || _laserLength > 0.0f || !_lockEndObject.id.isNull())) {
             float distance = _laserLength > 0.0f ? _laserLength : prevRayPickResult.distance;
             updateRenderState(_renderStates[_currentRenderState], prevRayPickResult.type, distance, prevRayPickResult.objectID, prevRayPickResult.searchRay, false);
             disableRenderState(_defaultRenderStates[_currentRenderState].second);
@@ -233,9 +234,11 @@ void LaserPointer::setLaserLength(const float laserLength) {
     });
 }
 
-void LaserPointer::setLockEndUUID(QUuid objectID, const bool isOverlay) {
+void LaserPointer::setLockEndUUID(QUuid objectID, const bool isOverlay, const glm::mat4& offsetMat) {
     withWriteLock([&] {
-        _objectLockEnd = std::pair<QUuid, bool>(objectID, isOverlay);
+        _lockEndObject.id = objectID;
+        _lockEndObject.isOverlay = isOverlay;
+        _lockEndObject.offsetMat = offsetMat;
     });
 }
 

interface/src/raypick/LaserPointer.h

@@ -21,6 +21,12 @@
 
 class RayPickResult;
 
+struct LockEndObject {
+    QUuid id { QUuid() };
+    bool isOverlay { false };
+    glm::mat4 offsetMat { glm::mat4() };
+};
+
 class RenderState {
 
 public:
@@ -74,7 +80,7 @@ public:
 
     void setPrecisionPicking(const bool precisionPicking);
     void setLaserLength(const float laserLength);
-    void setLockEndUUID(QUuid objectID, const bool isOverlay);
+    void setLockEndUUID(QUuid objectID, const bool isOverlay, const glm::mat4& offsetMat = glm::mat4());
 
     void setIgnoreItems(const QVector<QUuid>& ignoreItems) const;
     void setIncludeItems(const QVector<QUuid>& includeItems) const;
@@ -91,7 +97,7 @@ private:
     bool _centerEndY;
     bool _lockEnd;
     bool _distanceScaleEnd;
-    std::pair<QUuid, bool> _objectLockEnd { std::pair<QUuid, bool>(QUuid(), false)};
+    LockEndObject _lockEndObject;
 
     const QUuid _rayPickUID;
 

interface/src/raypick/LaserPointerManager.cpp

@@ -113,9 +113,9 @@ void LaserPointerManager::setIncludeItems(const QUuid& uid, const QVector<QUuid>
     }
 }
 
-void LaserPointerManager::setLockEndUUID(const QUuid& uid, const QUuid& objectID, const bool isOverlay) const {
+void LaserPointerManager::setLockEndUUID(const QUuid& uid, const QUuid& objectID, const bool isOverlay, const glm::mat4& offsetMat) const {
     auto laserPointer = find(uid);
     if (laserPointer) {
-        laserPointer->setLockEndUUID(objectID, isOverlay);
+        laserPointer->setLockEndUUID(objectID, isOverlay, offsetMat);
     }
 }

interface/src/raypick/LaserPointerManager.h

@@ -39,7 +39,7 @@ public:
     void setIgnoreItems(const QUuid& uid, const QVector<QUuid>& ignoreEntities) const;
     void setIncludeItems(const QUuid& uid, const QVector<QUuid>& includeEntities) const;
 
-    void setLockEndUUID(const QUuid& uid, const QUuid& objectID, const bool isOverlay) const;
+    void setLockEndUUID(const QUuid& uid, const QUuid& objectID, const bool isOverlay, const glm::mat4& offsetMat = glm::mat4()) const;
 
     void update();
 

interface/src/raypick/LaserPointerScriptingInterface.h

@@ -35,7 +35,7 @@ public slots:
     Q_INVOKABLE void setIgnoreItems(const QUuid& uid, const QScriptValue& ignoreEntities) const;
     Q_INVOKABLE void setIncludeItems(const QUuid& uid, const QScriptValue& includeEntities) const;
 
-    Q_INVOKABLE void setLockEndUUID(const QUuid& uid, const QUuid& objectID, bool isOverlay) const { qApp->getLaserPointerManager().setLockEndUUID(uid, objectID, isOverlay); }
+    Q_INVOKABLE void setLockEndUUID(const QUuid& uid, const QUuid& objectID, bool isOverlay, const glm::mat4& offsetMat = glm::mat4()) const { qApp->getLaserPointerManager().setLockEndUUID(uid, objectID, isOverlay, offsetMat); }
 
 private:
     static RenderState buildRenderState(const QVariantMap& propMap);

scripts/system/controllers/controllerModules/farActionGrabEntity.js

@@ -13,12 +13,13 @@
    makeDispatcherModuleParameters, MSECS_PER_SEC, HAPTIC_PULSE_STRENGTH, HAPTIC_PULSE_DURATION,
    PICK_MAX_DISTANCE, COLORS_GRAB_SEARCHING_HALF_SQUEEZE, COLORS_GRAB_SEARCHING_FULL_SQUEEZE, COLORS_GRAB_DISTANCE_HOLD,
    DEFAULT_SEARCH_SPHERE_DISTANCE, TRIGGER_OFF_VALUE, TRIGGER_ON_VALUE, ZERO_VEC, ensureDynamic,
-   getControllerWorldLocation, projectOntoEntityXYPlane, ContextOverlay, HMD, Reticle, Overlays, isPointingAtUI
+   getControllerWorldLocation, projectOntoEntityXYPlane, ContextOverlay, HMD, Reticle, Overlays, isPointingAtUI, Xform, getEntityParents
 
 */
 
 Script.include("/~/system/libraries/controllerDispatcherUtils.js");
 Script.include("/~/system/libraries/controllers.js");
+Script.include("/~/system/libraries/Xform.js");
 
 (function() {
     var PICK_WITH_HAND_RAY = true;
@@ -113,18 +114,71 @@ Script.include("/~/system/libraries/controllers.js");
     ];
 
     var MARGIN = 25;
+
+    function TargetObject(entityID, entityProps) {
+        this.entityID = entityID;
+        this.entityProps = entityProps;
+        this.targetEntityID = null;
+        this.targetEntityProps = null;
+        this.previousCollisionStatus = null;
+        this.madeDynamic = null;
+
+        this.makeDynamic = function() {
+            if (this.targetEntityID) {
+                var newProps = {
+                    dynamic: true,
+                    collisionless: true
+                };
+                this.previousCollisionStatus = this.targetEntityProps.collisionless;
+                Entities.editEntity(this.targetEntityID, newProps);
+                this.madeDynamic = true;
+            }
+        };
+
+        this.restoreTargetEntityOriginalProps = function() {
+            if (this.madeDynamic) {
+                var props = {};
+                props.dynamic = false;
+                props.collisionless = this.previousCollisionStatus;
+                var zeroVector = {x: 0, y: 0, z:0};
+                props.localVelocity = zeroVector;
+                props.localRotation = zeroVector;
+                Entities.editEntity(this.targetEntityID, props);
+            }
+        };
+
+        this.getTargetEntity = function() {
+            var parentPropsLength = this.parentProps.length;
+            if (parentPropsLength !== 0) {
+                var targetEntity = {
+                    id: this.parentProps[parentPropsLength - 1].id,
+                    props: this.parentProps[parentPropsLength - 1]};
+                this.targetEntityID = targetEntity.id;
+                this.targetEntityProps = targetEntity.props;
+                return targetEntity;
+            }
+            this.targetEntityID = this.entityID;
+            this.targetEntityProps = this.entityProps;
+            return {
+                id: this.entityID,
+                props: this.entityProps};
+        };
+    }
+
     function FarActionGrabEntity(hand) {
         this.hand = hand;
         this.grabbedThingID = null;
+        this.targetObject = null;
         this.actionID = null; // action this script created...
+        this.entityToLockOnto = null;
         this.entityWithContextOverlay = false;
         this.contextOverlayTimer = false;
         this.previousCollisionStatus = false;
+        this.locked = false;
         this.reticleMinX = MARGIN;
         this.reticleMaxX;
         this.reticleMinY = MARGIN;
         this.reticleMaxY;
-        this.madeDynamic = false;
 
         var ACTION_TTL = 15; // seconds
 
@@ -158,9 +212,25 @@ Script.include("/~/system/libraries/controllers.js");
             LaserPointers.enableLaserPointer(laserPointerID);
             LaserPointers.setRenderState(laserPointerID, mode);
             if (this.distanceHolding || this.distanceRotating) {
-                LaserPointers.setLockEndUUID(laserPointerID, this.grabbedThingID, this.grabbedIsOverlay);
+                if (!this.locked) {
+                    // calculate offset
+                    var targetProps = Entities.getEntityProperties(this.targetObject.entityID, [
+                        "position",
+                        "rotation"
+                    ]);
+                    var zeroVector = { x: 0, y: 0, z:0, w: 0 };
+                    var intersection = controllerData.rayPicks[this.hand].intersection;
+                    var intersectionMat = new Xform(zeroVector, intersection);
+                    var modelMat = new Xform(targetProps.rotation, targetProps.position);
+                    var modelMatInv = modelMat.inv();
+                    var xformMat = Xform.mul(modelMatInv, intersectionMat);
+                    var offsetMat = Mat4.createFromRotAndTrans(xformMat.rot, xformMat.pos);
+                    LaserPointers.setLockEndUUID(laserPointerID, this.targetObject.entityID, this.grabbedIsOverlay, offsetMat);
+                    this.locked = true;
+                }
             } else {
                 LaserPointers.setLockEndUUID(laserPointerID, null, false);
+                this.locked = false;
             }
         };
 
@@ -339,21 +409,15 @@ Script.include("/~/system/libraries/controllers.js");
 
             var args = [this.hand === RIGHT_HAND ? "right" : "left", MyAvatar.sessionUUID];
             Entities.callEntityMethod(this.grabbedThingID, "releaseGrab", args);
-
+            if (this.targetObject) {
-            if (this.madeDynamic) {
+                this.targetObject.restoreTargetEntityOriginalProps();
-                var props = {};
-                props.dynamic = false;
-                props.collisionless = this.previousCollisionStatus;
-                props.localVelocity = {x: 0, y: 0, z: 0};
-                props.localRotation = {x: 0, y: 0, z: 0};
-                Entities.editEntity(this.grabbedThingID, props);
-                this.madeDynamic = false;
             }
             this.actionID = null;
             this.grabbedThingID = null;
+            this.targetObject = null;
         };
 
-         this.updateRecommendedArea = function() {
+        this.updateRecommendedArea = function() {
             var dims = Controller.getViewportDimensions();
             this.reticleMaxX = dims.x - MARGIN;
             this.reticleMaxY = dims.y - MARGIN;
@@ -503,17 +567,18 @@ Script.include("/~/system/libraries/controllers.js");
                             "userData", "locked", "type"
                         ]);
 
+                        this.targetObject = new TargetObject(entityID, targetProps);
+                        this.targetObject.parentProps = getEntityParents(targetProps);
                         if (entityID !== this.entityWithContextOverlay) {
                             this.destroyContextOverlay();
                         }
+                        var targetEntity = this.targetObject.getTargetEntity();
+                        entityID = targetEntity.id;
+                        targetProps = targetEntity.props;
 
                         if (entityIsGrabbable(targetProps)) {
                             if (!entityIsDistanceGrabbable(targetProps)) {
-                                targetProps.dynamic = true;
+                                this.targetObject.makeDynamic();
-                                this.previousCollisionStatus = targetProps.collisionless;
-                                targetProps.collisionless = true;
-                                Entities.editEntity(entityID, targetProps);
-                                this.madeDynamic = true;
                             }
 
                             if (!this.distanceRotating) {

scripts/system/libraries/controllerDispatcherUtils.js

@@ -35,12 +35,14 @@
    propsArePhysical:true,
    controllerDispatcherPluginsNeedSort:true,
    projectOntoXYPlane:true,
+   getChildrenProps:true,
    projectOntoEntityXYPlane:true,
    projectOntoOverlayXYPlane:true,
    entityHasActions:true,
    ensureDynamic:true,
    findGroupParent:true,
    BUMPER_ON_VALUE:true,
+   getEntityParents:true,
    findHandChildEntities:true,
    TEAR_AWAY_DISTANCE:true,
    TEAR_AWAY_COUNT:true,
@@ -306,6 +308,23 @@ findGroupParent = function (controllerData, targetProps) {
     return targetProps;
 };
 
+getEntityParents = function(targetProps) {
+    var parentProperties = [];
+    while (targetProps.parentID &&
+           targetProps.parentID !== Uuid.NULL &&
+           Entities.getNestableType(targetProps.parentID) == "entity") {
+        var parentProps = Entities.getEntityProperties(targetProps.parentID, DISPATCHER_PROPERTIES);
+        if (!parentProps) {
+            break;
+        }
+        parentProps.id = targetProps.parentID;
+        targetProps = parentProps;
+        parentProperties.push(parentProps);
+    }
+
+    return parentProperties;
+};
+
 
 findHandChildEntities = function(hand) {
     // find children of avatar's hand joint