Merge pull request #6870 from julenka/issue/6634

Fix hand rays turning off when near a grabbable that's not visible

Author: Julia Schwarz

Assets/MixedRealityToolkit.SDK/Features/UX/Scripts/Pointers/SpherePointer.cs

@@ -1,7 +1,7 @@
 // Copyright (c) Microsoft Corporation. All rights reserved.
 // Licensed under the MIT License. See LICENSE in the project root for license information.
 
-using System;
+using System.Collections.Generic;
 using Microsoft.MixedReality.Toolkit.Physics;
 using Microsoft.MixedReality.Toolkit.Utilities;
 using UnityEngine;
@@ -65,6 +65,20 @@ public class SpherePointer : BaseControllerPointer, IMixedRealityNearPointer
         /// </summary>
         public int SceneQueryBufferSize => sceneQueryBufferSize;
 
+        [SerializeField]
+        [Tooltip("Whether to ignore colliders that may be near the pointer, but not actually in the visual FOV. This can prevent accidental grabs, and will allow hand rays to turn on when you may be near a grabbable but cannot see it. Visual FOV is defined by cone centered about display center, radius equal to half display height.")]
+        private bool ignoreCollidersNotInFOV = true;
+        /// <summary>
+        /// Whether to ignore colliders that may be near the pointer, but not actually in the visual FOV.
+        /// This can prevent accidental grabs, and will allow hand rays to turn on when you may be near 
+        /// a grabbable but cannot see it. Visual FOV is defined by cone centered about display center, 
+        /// radius equal to half display height.
+        /// </summary>
+        public bool IgnoreCollidersNotInFOV
+        {
+            get => ignoreCollidersNotInFOV;
+            set => ignoreCollidersNotInFOV = value;
+        }
 
         private SpherePointerQueryInfo queryBufferNearObjectRadius;
         private SpherePointerQueryInfo queryBufferInteractionRadius;
@@ -124,15 +138,15 @@ public override void OnPreSceneQuery()
 
                 for (int i = 0; i < layerMasks.Length; i++)
                 {
-                    if (queryBufferNearObjectRadius.TryUpdateQueryBufferForLayerMask(layerMasks[i], pointerPosition, triggerInteraction))
+                    if (queryBufferNearObjectRadius.TryUpdateQueryBufferForLayerMask(layerMasks[i], pointerPosition, triggerInteraction, ignoreCollidersNotInFOV))
                     {
                         break;
                     }
                 }
 
                 for (int i = 0; i < layerMasks.Length; i++)
                 {
-                    if (queryBufferInteractionRadius.TryUpdateQueryBufferForLayerMask(layerMasks[i], pointerPosition, triggerInteraction))
+                    if (queryBufferInteractionRadius.TryUpdateQueryBufferForLayerMask(layerMasks[i], pointerPosition, triggerInteraction, ignoreCollidersNotInFOV))
                     {
                         break;
                     }
@@ -213,6 +227,15 @@ public bool TryGetNormalToNearestSurface(out Vector3 normal)
         /// </summary>
         private class SpherePointerQueryInfo
         {
+            // List of corners shared across all sphere pointer query instances --
+            // used to store list of corners for a bounds. Shared and static
+            // to avoid allocating memory each frame
+            private static List<Vector3> corners = new List<Vector3>();
+            // Help to clear caches when new frame runs
+            static private int lastCalculatedFrame = -1;
+            // Map from grabbable => is the grabbable in FOV for this frame. Cleared every frame
+            private static Dictionary<Collider, bool> colliderCache = new Dictionary<Collider, bool>();
+
             /// <summary>
             /// How many colliders are near the point from the latest call to TryUpdateQueryBufferForLayerMask 
             /// </summary>
@@ -240,7 +263,18 @@ public SpherePointerQueryInfo(int bufferSize, float radius)
                 queryRadius = radius;
             }
 
-            public bool TryUpdateQueryBufferForLayerMask(LayerMask layerMask, Vector3 pointerPosition, QueryTriggerInteraction triggerInteraction)
+            /// <summary>
+            /// Intended to be called once per frame, this method performs a sphere intersection test against
+            /// all colliders in the layers defined by layerMask at the given pointer position.
+            /// All colliders intersecting the sphere at queryRadius and pointerPosition are stored in queryBuffer,
+            /// and the first grabbable in the list of returned colliders is stored.
+            /// Also provides an option to ignore colliders that are not visible.
+            /// </summary>
+            /// <param name="layerMask">Filter to only perform sphere cast on these layers.</param>
+            /// <param name="pointerPosition">The position of the pointer to query against.</param>
+            /// <param name="triggerInteraction">Passed along to the OverlapSphereNonAlloc call.</param>
+            /// <param name="ignoreCollidersNotInFOV">Whether to ignore colliders that are not visible.</param>
+            public bool TryUpdateQueryBufferForLayerMask(LayerMask layerMask, Vector3 pointerPosition, QueryTriggerInteraction triggerInteraction, bool ignoreCollidersNotInFOV)
             {
                 grabbable = null;
                 numColliders = UnityEngine.Physics.OverlapSphereNonAlloc(
@@ -257,14 +291,107 @@ public bool TryUpdateQueryBufferForLayerMask(LayerMask layerMask, Vector3 pointe
 
                 for (int i = 0; i < numColliders; i++)
                 {
-                    grabbable = queryBuffer[i].GetComponent<NearInteractionGrabbable>();
+                    Collider collider = queryBuffer[i];
+                    grabbable = collider.GetComponent<NearInteractionGrabbable>();
+                    if (grabbable != null)
+                    {
+                        if (ignoreCollidersNotInFOV)
+                        {
+                            if (!isInFOVCone(collider))
+                            {
+                                // Additional check: is grabbable in the camera frustrum
+                                // We do this so that if grabbable is not visible it is not accidentally grabbed
+                                // Also to not turn off the hand ray if hand is near a grabbable that's not actually visible
+                                grabbable = null;
+                            }
+                        }   
+                    }
+
                     if (grabbable != null)
                     {
                         return true;
                     }
                 }
                 return false;
             }
+
+
+            /// <summary>
+            /// Returns true if a collider's bounds is within the camera FOV
+            /// </summary>
+            /// <param name="myCollider">The collider to test</param>
+            private bool isInFOVCone(Collider myCollider)
+            {
+                if (lastCalculatedFrame != Time.frameCount)
+                {
+                    colliderCache.Clear();
+                    lastCalculatedFrame = Time.frameCount;
+                }
+                if (colliderCache.TryGetValue(myCollider, out bool result))
+                {
+                    return result;
+                }
+
+                corners.Clear();
+                BoundsExtensions.GetColliderBoundsPoints(myCollider, corners, 0);
+
+                float xMin = float.MaxValue, yMin = float.MaxValue, zMin = float.MaxValue;
+                float xMax = float.MinValue, yMax = float.MinValue, zMax = float.MinValue;
+                for (int i = 0; i < corners.Count; i++)
+                {
+                    var corner = corners[i];
+                    if (isPointInFOVCone(corner, 0))
+                    {
+                        colliderCache.Add(myCollider, true);
+                        return true;
+                    }
+
+                    xMin = Mathf.Min(xMin, corner.x);
+                    yMin = Mathf.Min(yMin, corner.y);
+                    zMin = Mathf.Min(zMin, corner.z);
+                    xMax = Mathf.Max(xMax, corner.x);
+                    yMax = Mathf.Max(yMax, corner.y);
+                    zMax = Mathf.Max(zMax, corner.z);
+                }
+
+                // edge case: check if camera is inside the entire bounds of the collider;
+                // Consider simplifying to myCollider.bounds.Contains(CameraCache.main.transform.position)
+                var cameraPos = CameraCache.Main.transform.position;
+                result = xMin <= cameraPos.x && cameraPos.x <= xMax 
+                    && yMin <= cameraPos.y && cameraPos.y <= yMax
+                    && zMin <= cameraPos.z && cameraPos.z <= zMax;
+                colliderCache.Add(myCollider, result);
+                return result;
+            }
+
+            /// <summary>
+            /// Returns true if a point is in the a cone inscribed into the 
+            /// Camera's frustrum. The cone is inscribed to match the vertical height of the camera's
+            /// FOV. By default, the cone's tip is "chopped off" by an amount defined by minDist. 
+            /// The cone's height is given by maxDist.
+            /// </summary>
+            /// <param name="point">Point to test</param>
+            /// <param name="coneAngleBufferDegrees">Degrees to expand the cone by.</param>
+            /// <param name="minDist">Point must be at least this far away (along the camera forward) from camera. </param>
+            /// <param name="maxDist">Point must be at most this far away (along camera forward) from camera. </param>
+            private static bool isPointInFOVCone(
+                Vector3 point, 
+                float coneAngleBufferDegrees = 0,
+                float minDist = 0.05f,
+                float maxDist = 100f)
+            {
+                Camera mainCam = CameraCache.Main;
+                var cameraToPoint = point - mainCam.transform.position;
+                var pointCameraDist = Vector3.Dot(mainCam.transform.forward, cameraToPoint);
+                if (pointCameraDist < minDist || pointCameraDist > maxDist)
+                {
+                    return false;
+                }
+                var verticalFOV = mainCam.fieldOfView + coneAngleBufferDegrees;
+                var degrees = Mathf.Acos(pointCameraDist / cameraToPoint.magnitude) * Mathf.Rad2Deg;
+                return degrees < verticalFOV * 0.5f;
+            }
+
             /// <summary>
             /// Returns true if any of the objects inside QueryBuffer contain a grabbable
             /// </summary>

Assets/MixedRealityToolkit.SDK/Inspectors/UX/Pointers/SpherePointerInspector.cs

@@ -15,6 +15,8 @@ public class SpherePointerInspector : BaseControllerPointerInspector
         private SerializedProperty grabLayerMasks;
         private SerializedProperty triggerInteraction;
         private SerializedProperty sceneQueryBufferSize;
+        private SerializedProperty ignoreCollidersNotInFOV;
+
 
         private bool spherePointerFoldout = true;
 
@@ -27,6 +29,7 @@ protected override void OnEnable()
             nearObjectMargin = serializedObject.FindProperty("nearObjectMargin");
             grabLayerMasks = serializedObject.FindProperty("grabLayerMasks");
             triggerInteraction = serializedObject.FindProperty("triggerInteraction");
+            ignoreCollidersNotInFOV = serializedObject.FindProperty("ignoreCollidersNotInFOV");
         }
 
         public override void OnInspectorGUI()
@@ -46,6 +49,7 @@ public override void OnInspectorGUI()
                     EditorGUILayout.PropertyField(nearObjectMargin);
                     EditorGUILayout.PropertyField(triggerInteraction);
                     EditorGUILayout.PropertyField(grabLayerMasks, true);
+                    EditorGUILayout.PropertyField(ignoreCollidersNotInFOV);
                 }
             }
 

Assets/MixedRealityToolkit.Tests/PlayModeTests/PointerTests.cs

@@ -38,6 +38,55 @@ public void TearDown()
 
         #region Tests
 
+        /// <summary>
+        /// Tests that sphere pointer grabs object when hand is insize a giant grabbable
+        /// </summary>
+        [UnityTest]
+        public IEnumerator TestSpherePointerInsideGrabbable()
+        {
+            var cube = GameObject.CreatePrimitive(PrimitiveType.Cube);
+            cube.AddComponent<NearInteractionGrabbable>();
+            var rightHand = new TestHand(Handedness.Right);
+            yield return rightHand.Show(Vector3.zero);
+            yield return PlayModeTestUtilities.WaitForInputSystemUpdate();
+
+            var spherePointer = PointerUtils.GetPointer<SpherePointer>(Handedness.Right);
+            Assert.IsNotNull(spherePointer, "Right hand does not have a sphere pointer");
+            Assert.IsTrue(spherePointer.IsInteractionEnabled, "Sphere pointer should be enabled because it is near grabbable cube and visible, even if inside a giant cube.");
+            GameObject.Destroy(cube);
+        }
+
+        /// <summary>
+        /// Tests that sphere pointer behaves correctly when hand is near grabbable
+        /// </summary>
+        [UnityTest]
+        public IEnumerator TestSpherePointerNearGrabbable()
+        {
+            var cube = GameObject.CreatePrimitive(PrimitiveType.Cube);
+            cube.AddComponent<NearInteractionGrabbable>();
+            cube.transform.position = Vector3.forward;
+            cube.transform.localScale = Vector3.one * 0.1f;
+
+            var rightHand = new TestHand(Handedness.Right);
+            yield return rightHand.Show(Vector3.forward);
+            yield return PlayModeTestUtilities.WaitForInputSystemUpdate();
+
+            var spherePointer = PointerUtils.GetPointer<SpherePointer>(Handedness.Right);
+            Assert.IsNotNull(spherePointer, "Right hand does not have a sphere pointer");
+            Assert.IsTrue(spherePointer.IsInteractionEnabled, "Sphere pointer should be enabled because it is near grabbable cube and visible.");
+            
+            // Move forward so that cube is no longer visible
+            CameraCache.Main.transform.Translate(Vector3.up * 10);
+            yield return PlayModeTestUtilities.WaitForInputSystemUpdate();
+            Assert.IsFalse(spherePointer.IsInteractionEnabled, "Sphere pointer should NOT be enabled because hand is near grabbable but the grabbable is not visible.");
+
+            // Move camera back so that cube is visible again
+            CameraCache.Main.transform.Translate(Vector3.up * -10f);
+            yield return PlayModeTestUtilities.WaitForInputSystemUpdate();
+            Assert.IsTrue(spherePointer.IsInteractionEnabled, "Sphere pointer should be enabled because it is near grabbable cube and visible.");
+            GameObject.Destroy(cube);
+        }
+
         /// <summary>
         /// Tests that right after being instantiated, the pointer's direction 
         /// is in the same general direction as the forward direction of the camera

Documentation/Images/Input/Pointers/SpherePointer_VisualFOV.png

@@ -97,15 +97,21 @@ Useful Poke Pointer properties:
 
 The *[SpherePointer](xref:Microsoft.MixedReality.Toolkit.Input.SpherePointer)* uses [UnityEngine.Physics.OverlapSphere](https://docs.unity3d.com/ScriptReference/Physics.OverlapSphere.html) in order to identify the closest [`NearInteractionGrabbable`](xref:Microsoft.MixedReality.Toolkit.Input.NearInteractionGrabbable) object for interaction, which is useful for "grabbable" input like the `ManipulationHandler`. Similar to the [`PokePointer`](xref:Microsoft.MixedReality.Toolkit.Input.PokePointer)/[`NearInteractionTouchable`](xref:Microsoft.MixedReality.Toolkit.Input.NearInteractionTouchable) functional pair, in order to be interactable with the Sphere Pointer, the game object must contain a component that is the [`NearInteractionGrabbable`](xref:Microsoft.MixedReality.Toolkit.Input.NearInteractionGrabbable) script.
 
+<img src="../../Documentation/Images/Pointers/MRTK_GrabPointer.jpg" width="400">
+
+
 Useful Sphere Pointer properties:
 
 - *Sphere Cast Radius*: The radius for the sphere used to query for grabbable objects.
 - *Grab Layer Masks* - A prioritized array of LayerMasks to determine which possible GameObjects the pointer can interact with and the order of interaction to attempt. Note that a GameObject must also have a `NearInteractionGrabbable` to interact with a SpherePointer.
-
 > [!NOTE]
 > The Spatial Awareness layer is disabled in the default GrabPointer prefab provided by MRTK. This is done to reduce performance impact of doing a sphere overlap query with the spatial mesh. You can enable this by modifying the GrabPointer prefab.
+- *Ignore Colliders Not in FOV* - Whether to ignore colliders that may be near the pointer, but not actually in the visual FOV.
+This can prevent accidental grabs, and will allow hand rays to turn on when you may be near 
+a grabbable but cannot see it. The *Visual FOV* is defined via a cone instead of the the typical frustum for performance reasons. This cone is centered and oriented the same as the camera's frustum with a radius equal to half display height(or vertical FOV). 
+
+<img src="../../Documentation/Images/Input/Pointers/SpherePointer_VisualFOV.png" width="200">
 
-<img src="../../Documentation/Images/Pointers/MRTK_GrabPointer.jpg" width="400">
 
 #### Teleport pointers