Implement smooth visualizer unlock (#139)

* Clean up unused parameter

* Implement smooth unlock

Runtime/Input/Controllers/BaseControllerVisualizer.cs

@@ -21,6 +21,9 @@ public abstract class BaseControllerVisualizer : ControllerPoseSynchronizer, ICo
         /// <inheritdoc />
         public GameObject GameObject => gameObject;
 
+        /// <inheritdoc />
+        public Pose SourcePose { get; private set; }
+
         /// <inheritdoc />
         public bool OverrideSourcePose { get; set; }
 
@@ -41,6 +44,8 @@ public Transform GripPose
         /// <inheritdoc />
         public override void OnSourcePoseChanged(SourcePoseEventData<Pose> eventData)
         {
+            SourcePose = eventData.SourceData;
+
             if (OverrideSourcePose)
             {
                 return;
@@ -52,6 +57,8 @@ public override void OnSourcePoseChanged(SourcePoseEventData<Pose> eventData)
         /// <inheritdoc />
         public override void OnSourcePoseChanged(SourcePoseEventData<Quaternion> eventData)
         {
+            SourcePose = new Pose(SourcePose.position, eventData.SourceData);
+
             if (OverrideSourcePose)
             {
                 return;
@@ -63,6 +70,8 @@ public override void OnSourcePoseChanged(SourcePoseEventData<Quaternion> eventDa
         /// <inheritdoc />
         public override void OnSourcePoseChanged(SourcePoseEventData<Vector2> eventData)
         {
+            SourcePose = new Pose(eventData.SourceData, SourcePose.rotation);
+
             if (OverrideSourcePose)
             {
                 return;
@@ -74,6 +83,8 @@ public override void OnSourcePoseChanged(SourcePoseEventData<Vector2> eventData)
         /// <inheritdoc />
         public override void OnSourcePoseChanged(SourcePoseEventData<Vector3> eventData)
         {
+            SourcePose = new Pose(eventData.SourceData, SourcePose.rotation);
+
             if (OverrideSourcePose)
             {
                 return;

Runtime/Input/Controllers/IControllerVisualizer.cs

@@ -18,6 +18,12 @@ public interface IControllerVisualizer : IControllerPoseSynchronizer
         /// </remarks>
         GameObject GameObject { get; }
 
+        /// <summary>
+        /// This is the actual pose of this controller, regardless of <see cref="OverrideSourcePose"/>
+        /// and the <see cref="IControllerPoseSynchronizer.PoseDriver"/> pose.
+        /// </summary>
+        Pose SourcePose { get; }
+
         /// <summary>
         /// If set, the <see cref="IControllerPoseSynchronizer.PoseDriver"/>'s pose in the scene
         /// is override and the actual <see cref="IController.InputSource"/> pose is ignored.

Runtime/Input/InteractionBehaviours/LockControllerVisualizerBehaviour.cs

@@ -33,6 +33,7 @@ public class LockControllerVisualizerBehaviour : BaseInteractionBehaviour
         private float syncDuration = 1f;
 
         private readonly Dictionary<IControllerVisualizer, bool> lockedVisualizers = new();
+        private readonly Dictionary<IControllerVisualizer, bool> pendingUnlockVisualizers = new();
         private readonly Dictionary<IControllerVisualizer, Pose> smoothingStartPose = new();
         private readonly Dictionary<IControllerVisualizer, float> smoothingStartTime = new();
         private readonly Dictionary<IControllerVisualizer, float> smoothingProgress = new();
@@ -50,15 +51,32 @@ protected override void Update()
 
             foreach (var visualizer in visualizers)
             {
-                var shouldLock = HasFinishedSmoothTransition(visualizer, lockPose);
-
-                if (!shouldLock)
+                if (pendingUnlockVisualizers.TryGetValue(visualizer, out _))
                 {
-                    lockPose.position = Vector3.Slerp(smoothingStartPose[visualizer].position, lockPose.position, smoothingProgress[visualizer]);
-                    lockPose.rotation = Quaternion.Slerp(smoothingStartPose[visualizer].rotation, lockPose.rotation, smoothingProgress[visualizer]);
+                    var finishedUnlock = HasFinishedSmoothTransition(pendingUnlockVisualizers, visualizer);
+                    if (finishedUnlock)
+                    {
+                        CleanUpVisualizer(visualizer);
+                        continue;
+                    }
+
+                    var unlockPose = visualizer.SourcePose;
+                    unlockPose.position = Vector3.Slerp(smoothingStartPose[visualizer].position, unlockPose.position, smoothingProgress[visualizer]);
+                    unlockPose.rotation = Quaternion.Slerp(smoothingStartPose[visualizer].rotation, unlockPose.rotation, smoothingProgress[visualizer]);
+                    visualizer.PoseDriver.SetPositionAndRotation(unlockPose.position, unlockPose.rotation);
                 }
+                else
+                {
+                    var shouldLock = HasFinishedSmoothTransition(lockedVisualizers, visualizer);
 
-                visualizer.PoseDriver.SetPositionAndRotation(lockPose.position, lockPose.rotation);
+                    if (!shouldLock)
+                    {
+                        lockPose.position = Vector3.Slerp(smoothingStartPose[visualizer].position, lockPose.position, smoothingProgress[visualizer]);
+                        lockPose.rotation = Quaternion.Slerp(smoothingStartPose[visualizer].rotation, lockPose.rotation, smoothingProgress[visualizer]);
+                    }
+
+                    visualizer.PoseDriver.SetPositionAndRotation(lockPose.position, lockPose.rotation);
+                }
             }
         }
 
@@ -120,19 +138,34 @@ private void LockVisualizer(IControllerVisualizer visualizer)
         }
 
         private void UnlockVisualizer(IControllerVisualizer visualizer)
+        {
+            if (!smoothSyncPose)
+            {
+                CleanUpVisualizer(visualizer);
+                return;
+            }
+
+            pendingUnlockVisualizers.EnsureDictionaryItem(visualizer, false, true);
+            smoothingStartPose.EnsureDictionaryItem(visualizer, GetLockPose(), true);
+            smoothingStartTime.EnsureDictionaryItem(visualizer, Time.time, true);
+            smoothingProgress.EnsureDictionaryItem(visualizer, 0f, true);
+        }
+
+        private void CleanUpVisualizer(IControllerVisualizer visualizer)
         {
             lockedVisualizers.SafeRemoveDictionaryItem(visualizer);
             smoothingStartPose.SafeRemoveDictionaryItem(visualizer);
             smoothingStartTime.SafeRemoveDictionaryItem(visualizer);
             smoothingProgress.SafeRemoveDictionaryItem(visualizer);
+            pendingUnlockVisualizers.SafeRemoveDictionaryItem(visualizer);
             visualizer.OverrideSourcePose = false;
         }
 
         private Pose GetLockPose() => new Pose(transform.TransformPoint(localOffsetPose.position), transform.rotation * Quaternion.Euler(localOffsetPose.rotation.eulerAngles));
 
-        private bool HasFinishedSmoothTransition(IControllerVisualizer visualizer, Pose snapPose)
+        private bool HasFinishedSmoothTransition(Dictionary<IControllerVisualizer, bool> smoothingStateDictionary, IControllerVisualizer visualizer)
         {
-            if (lockedVisualizers[visualizer])
+            if (smoothingStateDictionary[visualizer])
             {
                 return true;
             }
@@ -145,7 +178,7 @@ private bool HasFinishedSmoothTransition(IControllerVisualizer visualizer, Pose
                 return false;
             }
 
-            lockedVisualizers[visualizer] = true;
+            smoothingStateDictionary[visualizer] = true;
             return true;
         }
     }