fixed farGrab parent grabbing and laser LockEndUUID

interface/src/raypick/LaserPointer.cpp

@@ -25,7 +25,7 @@ LaserPointer::LaserPointer(const QVariant& rayProps, const RenderStateMap& rende
     _distanceScaleEnd(distanceScaleEnd),
     _rayPickUID(DependencyManager::get<RayPickScriptingInterface>()->createRayPick(rayProps))
 {
-    
+    _offsetMat = glm::mat4();
 
     for (auto& state : _renderStates) {
         if (!enabled || state.first != _currentRenderState) {
@@ -134,8 +134,10 @@ void LaserPointer::updateRenderState(const RenderState& renderState, const Inter
                 registrationPoint = glm::vec3(0.5f);
             } else {
                 EntityItemProperties props = DependencyManager::get<EntityScriptingInterface>()->getEntityProperties(_objectLockEnd.first);
-                pos = props.getPosition();
+                glm::mat4 entityMat = createMatFromQuatAndPos(props.getRotation(), props.getPosition());
-                rot = props.getRotation();
+                glm::mat4 finalPosAndRotMat = entityMat * _offsetMat;
+                pos = extractTranslation(finalPosAndRotMat);
+                rot = glmExtractRotation(finalPosAndRotMat);
                 dim = props.getDimensions();
                 registrationPoint = props.getRegistrationPoint();
             }
@@ -233,9 +235,10 @@ void LaserPointer::setLaserLength(const float laserLength) {
     });
 }
 
-void LaserPointer::setLockEndUUID(QUuid objectID, const bool isOverlay, const glm::mat4& offset) {
+void LaserPointer::setLockEndUUID(QUuid objectID, const bool isOverlay, const glm::mat4& offsetMat) {
     withWriteLock([&] {
         _objectLockEnd = std::pair<QUuid, bool>(objectID, isOverlay);
+        _offsetMat = offsetMat;
     });
 }
 

interface/src/raypick/LaserPointer.h

@@ -74,7 +74,7 @@ public:
 
     void setPrecisionPicking(const bool precisionPicking);
     void setLaserLength(const float laserLength);
-    void setLockEndUUID(QUuid objectID, const bool isOverlay, const glm::mat4& offset = glm::mat4());
+    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,6 +91,7 @@ private:
     bool _centerEndY;
     bool _lockEnd;
     bool _distanceScaleEnd;
+    glm::mat4 _offsetMat;
     std::pair<QUuid, bool> _objectLockEnd { std::pair<QUuid, bool>(QUuid(), false)};
 
     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,13 +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.hs");
+Script.include("/~/system/libraries/Xform.js");
 
 (function() {
     var PICK_WITH_HAND_RAY = true;
@@ -115,32 +115,53 @@ Script.include("/~/system/libraries/Xform.hs");
 
     var MARGIN = 25;
 
-    function TargetObject(entityID) {
+    function TargetObject(entityID, entityProps) {
         this.entityID = entityID;
-        this.entityProps = null;
+        this.entityProps = entityProps;
-        this.parentID = null;
+        this.targetEntityID = null;
-        this.parentProps = [];
+        this.targetEntityProps = null;
-        this.childrenProps = [];
+        this.previousCollisionStatus = null;
-        this.parentsCollisionStatus = [];
-        this.childrenCollisionStatus = [];
         this.madeDynamic = null;
-        this.parentEntityProps = 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.saveCollisionStatus = function() {
+        this.restoreTargetEntityOriginalProps = function() {
-        };
+            if (this.madeDynamic) {
-
+                var props = {};
-        this.restoreEntitiesProperties = function() {
+                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) {
-                return this.parentProps[parentPropsLength].id;
+                var targetEntity = {
+                    id: this.parentProps[parentPropsLength - 1].id,
+                    props: this.parentProps[parentPropsLength - 1]};
+                this.targetEntityID = targetEntity.id;
+                this.targetEntityProps = targetEntity.props;
+                return targetEntity;
             }
-            return this.entityID;
+            this.targetEntityID = this.entityID;
+            this.targetEntityProps = this.entityProps;
+            return {
+                id: this.entityID,
+                props: this.entityProps};
         };
     }
 
@@ -153,11 +174,11 @@ Script.include("/~/system/libraries/Xform.hs");
         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
 
@@ -191,10 +212,25 @@ Script.include("/~/system/libraries/Xform.hs");
             LaserPointers.enableLaserPointer(laserPointerID);
             LaserPointers.setRenderState(laserPointerID, mode);
             if (this.distanceHolding || this.distanceRotating) {
-                // calculate offset
+                if (!this.locked) {
-                LaserPointers.setLockEndUUID(laserPointerID, this.entityToLockOnto, this.grabbedIsOverlay);
+                    // 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;
             }
         };
 
@@ -373,22 +409,15 @@ Script.include("/~/system/libraries/Xform.hs");
 
             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.entityToLockOnto = null;
+            this.targetObject = null;
         };
 
-         this.updateRecommendedArea = function() {
+        this.updateRecommendedArea = function() {
             var dims = Controller.getViewportDimensions();
             this.reticleMaxX = dims.x - MARGIN;
             this.reticleMaxY = dims.y - MARGIN;
@@ -532,30 +561,24 @@ Script.include("/~/system/libraries/Xform.hs");
                 if (rayPickInfo.type === RayPick.INTERSECTED_ENTITY) {
                     if (controllerData.triggerClicks[this.hand]) {
                         var entityID = rayPickInfo.objectID;
-                        this.entityToLockOnto = entityID;
                         var targetProps = Entities.getEntityProperties(entityID, [
                             "dynamic", "shapeType", "position",
                             "rotation", "dimensions", "density",
                             "userData", "locked", "type"
                         ]);
 
-                        this.targetObject = new TargetObject(entityID);
+                        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)) {
-                            var groupRootProps = findGroupParent(controllerData, targetProps);
-                            if (entityIsGrabbable(groupRootProps)) {
-                                targetProps = groupRootProps;
-                                entityID = targetProps.id;
-                            }
                             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

@@ -308,16 +308,6 @@ findGroupParent = function (controllerData, targetProps) {
     return targetProps;
 };
 
-getChildrenProps = function(entityID) {
-    var childrenProps = [];
-    var childrenIDs = Entities.getChildrenIDs(entityID);
-    for (var index = 0; index < childrenIDs.length; index++) {
-        var childProps = Entities.getEntityProperties(childrenIDs[index]);
-        childrenProps.push(childProps);
-    }
-    return childrenProps;
-};
-
 getEntityParents = function(targetProps) {
     var parentProperties = [];
     while (targetProps.parentID &&