Fixed objects drifting during Near Manipulation (#10294)

* Fixed objects drifting during Near Manipulation

* Updated unit tests, made near manipulation consistent to avoid weird offset behaviors

* missing changes

* reverted manipulation handler changes due to it being obsolete, fixed CI

* removed debug

Author: RogPodge

Assets/MRTK/SDK/Features/Input/Handlers/Manipulation/ManipulationHandler.cs

@@ -684,7 +684,9 @@ private void HandleTwoHandManipulationUpdated()
                 // the initial pointer pose; if far manipulation, a more complex
                 // look-rotation-based pointer pose is used.
                 MixedRealityPose pose = IsNearManipulation() ? new MixedRealityPose(GetPointersCentroid()) : GetAveragePointerPose();
-                targetTransform.Position = moveLogic.Update(pose, targetTransform.Rotation, targetTransform.Scale, true);
+
+                // The manipulation handler is not built to handle near manipulation properly, please use the object manipulator
+                targetTransform.Position = moveLogic.UpdateTransform(pose, targetTransform, true, false);
                 if (constraintOnMovement == MovementConstraintType.FixDistanceFromHead && moveConstraint != null)
                 {
                     moveConstraint.ApplyConstraint(ref targetTransform);
@@ -748,7 +750,9 @@ private void HandleOneHandMoveUpdated()
             }
 
             MixedRealityPose pointerPose = new MixedRealityPose(pointer.Position, pointer.Rotation);
-            targetTransform.Position = moveLogic.Update(pointerPose, targetTransform.Rotation, targetTransform.Scale, rotateInOneHandType != RotateInOneHandType.RotateAboutObjectCenter);
+
+            // The manipulation handler is not built to handle near manipulation properly, please use the object manipulator
+            targetTransform.Position = moveLogic.UpdateTransform(pointerPose, targetTransform, rotateInOneHandType != RotateInOneHandType.RotateAboutObjectCenter, false);
             if (constraintOnMovement == MovementConstraintType.FixDistanceFromHead && moveConstraint != null)
             {
                 moveConstraint.ApplyConstraint(ref targetTransform);

Assets/MRTK/SDK/Features/Input/Handlers/Manipulation/ManipulationMoveLogic.cs

@@ -2,6 +2,7 @@
 // Licensed under the MIT License.
 
 using Microsoft.MixedReality.Toolkit.Utilities;
+using System;
 using UnityEngine;
 
 namespace Microsoft.MixedReality.Toolkit.Physics
@@ -25,6 +26,9 @@ internal class ManipulationMoveLogic
         private Vector3 objectLocalGrabPoint;
         private Vector3 grabToObject;
 
+        private Vector3 objectLocalAttachPoint;
+        private Vector3 attachToObject;
+
         /// <summary>
         /// Setup function
         /// </summary>
@@ -37,17 +41,57 @@ public void Setup(MixedRealityPose pointerCentroidPose, Vector3 grabCentroid, Mi
             Quaternion worldToPointerRotation = Quaternion.Inverse(pointerCentroidPose.Rotation);
             pointerLocalGrabPoint = worldToPointerRotation * (grabCentroid - pointerCentroidPose.Position);
 
-            objectLocalGrabPoint = Quaternion.Inverse(objectPose.Rotation) * (grabCentroid - objectPose.Position);
-            objectLocalGrabPoint = objectLocalGrabPoint.Div(objectScale);
+            attachToObject = objectPose.Position - pointerCentroidPose.Position;
+            objectLocalAttachPoint = Quaternion.Inverse(objectPose.Rotation) * (pointerCentroidPose.Position - objectPose.Position);
+            objectLocalAttachPoint = objectLocalAttachPoint.Div(objectScale);
 
             grabToObject = objectPose.Position - grabCentroid;
+            objectLocalGrabPoint = Quaternion.Inverse(objectPose.Rotation) * (grabCentroid - objectPose.Position);
+            objectLocalGrabPoint = objectLocalGrabPoint.Div(objectScale);
         }
 
         /// <summary>
         /// Update the position based on input.
         /// </summary>
         /// <returns>A Vector3 describing the desired position</returns>
+        [Obsolete("This update function is out of date and does not properly support Near Manipulation. Use UpdateTransform instead")]
         public Vector3 Update(MixedRealityPose pointerCentroidPose, Quaternion objectRotation, Vector3 objectScale, bool usePointerRotation)
+        {
+            return FarManipulationUpdate(pointerCentroidPose, objectRotation, objectScale, usePointerRotation);
+        }
+
+        /// <summary>
+        /// Update the position based on input.
+        /// </summary>
+        /// <returns>A Vector3 describing the desired position</returns>
+        public Vector3 UpdateTransform(MixedRealityPose pointerCentroidPose, MixedRealityTransform currentTarget, bool isPointerAnchor, bool isNearManipulation)
+        {
+            if (isNearManipulation)
+            {
+                return NearManipulationUpdate(pointerCentroidPose, currentTarget);
+            }
+            else
+            {
+                return FarManipulationUpdate(pointerCentroidPose, currentTarget.Rotation, currentTarget.Scale, isPointerAnchor);
+            }
+        }
+
+        /// <summary>
+        /// Updates the position during near manipulation
+        /// </summary>
+        /// <returns>A Vector3 describing the desired position during near manipulation</returns>
+        private Vector3 NearManipulationUpdate(MixedRealityPose pointerCentroidPose, MixedRealityTransform currentTarget)
+        {
+            Vector3 scaledLocalAttach = Vector3.Scale(objectLocalAttachPoint, currentTarget.Scale);
+            Vector3 worldAttachPoint = currentTarget.Rotation * scaledLocalAttach + currentTarget.Position;
+            return currentTarget.Position + (pointerCentroidPose.Position - worldAttachPoint);
+        }
+
+        /// <summary>
+        /// Updates the position during far manipulation
+        /// </summary>
+        /// <returns>A Vector3 describing the desired position during far manipulation</returns>
+        private Vector3 FarManipulationUpdate(MixedRealityPose pointerCentroidPose, Quaternion objectRotation, Vector3 objectScale, bool isPointerAnchor)
         {
             float distanceRatio = 1.0f;
 
@@ -58,7 +102,7 @@ public Vector3 Update(MixedRealityPose pointerCentroidPose, Quaternion objectRot
                 distanceRatio = currentHandDistance / pointerRefDistance;
             }
 
-            if (usePointerRotation)
+            if (isPointerAnchor)
             {
                 Vector3 scaledGrabToObject = Vector3.Scale(objectLocalGrabPoint, objectScale);
                 Vector3 adjustedPointerToGrab = (pointerLocalGrabPoint * distanceRatio);

Assets/MRTK/SDK/Features/Input/Handlers/ObjectManipulator.cs

@@ -727,7 +727,7 @@ private void HandleTwoHandManipulationUpdated()
                 // the pointer pose; if far manipulation, a more complex
                 // look-rotation-based pointer pose is used.
                 MixedRealityPose pose = IsNearManipulation() ? new MixedRealityPose(GetPointersGrabPoint()) : GetPointersPose();
-                targetTransform.Position = moveLogic.Update(pose, targetTransform.Rotation, targetTransform.Scale, true);
+                targetTransform.Position = moveLogic.UpdateTransform(pose, targetTransform, true, IsNearManipulation());
                 if (EnableConstraints && constraintsManager != null)
                 {
                     constraintsManager.ApplyTranslationConstraints(ref targetTransform, false, IsNearManipulation());
@@ -778,7 +778,7 @@ private void HandleOneHandMoveUpdated()
 
             RotateInOneHandType rotateInOneHandType = isNearManipulation ? oneHandRotationModeNear : oneHandRotationModeFar;
             MixedRealityPose pointerPose = new MixedRealityPose(pointer.Position, pointer.Rotation);
-            targetTransform.Position = moveLogic.Update(pointerPose, targetTransform.Rotation, targetTransform.Scale, rotateInOneHandType != RotateInOneHandType.RotateAboutObjectCenter);
+            targetTransform.Position = moveLogic.UpdateTransform(pointerPose, targetTransform, rotateInOneHandType != RotateInOneHandType.RotateAboutObjectCenter, IsNearManipulation());
 
             if (EnableConstraints && constraintsManager != null)
             {