Wand.cs File Reference

Go to the source code of this file.

Classes
class	TiltFive.WandSettings
	Wand Settings encapsulates all configuration data used by the Wand's tracking runtime to compute the Wand Pose and apply it to the driven GameObject. More...
class	TiltFive.Wand
	The Wand API and runtime. More...
struct	TiltFive.Wand.WandPair
class	TiltFive.Wand.WandCore
	Internal Wand core runtime. More...

Namespaces
namespace	TiltFive

Typedefs
using	WandButton = TiltFive.Input.WandButton

Functions
	[instance initializer]

Variables
$	Glasses

Typedef Documentation

WandButton

using WandButton = TiltFive.Input.WandButton

Definition at line 28 of file Wand.cs.

Function Documentation

[instance initializer]()

GetLatestInputs.[instance initializer]

Variable Documentation

Glasses

$ Glasses

Definition at line 671 of file Wand.cs.

{
                previousState = currentState;
 
                try
                {
                    T5_ControllerState state = new T5_ControllerState();
 
                    var result = NativePlugin.GetControllerState(glassesHandle, controllerIndex, ref state);
 
                    currentState = (result == 0)
                        ? state
                        : (T5_ControllerState?)null;
                }
                catch (Exception e)
                {
                    currentState = null;
                    Log.Error(e.Message);
                }
            }
 
            public bool GetButton(WandButton button)
            {
                // If the wand isn't connected, GetButton() should return a default value of false.
                return currentState?.GetButton(button) ?? false;
            }
 
            public bool TryGetButton(WandButton button, out bool pressed)
            {
                pressed = currentState?.GetButton(button) ?? false;
 
                // If the wand isn't connected, TryGetButton() should fail.
                return currentState.HasValue;
            }
 
            public bool GetButtonDown(WandButton button)
            {
 
                // If the current wand state is null, the wand isn't connected.
                // If so, let the application assume the user isn't pressing the button currently.
                var pressed = currentState?.GetButton(button) ?? false;
 
                // If the previous wand state is null, the wand wasn't connected.
                // If so, let the application assume the user wasn't pressing the button last frame.
                var previouslyPressed = previousState?.GetButton(button) ?? false;
 
                // The wand could potentially connect while the user is holding a button, so just report the button state.
                if (!previousState.HasValue && currentState.HasValue)
                {
                    return pressed;
                }
                // Return true if the button is currently pressed, but was unpressed on the previous frame.
                return pressed && !previouslyPressed;
            }
 
            public bool TryGetButtonDown(WandButton button, out bool buttonDown)
            {
                // Even if this operation fails, give buttonDown a default value.
                buttonDown = GetButtonDown(button);
                return currentState.HasValue;
            }
 
            public bool GetButtonUp(WandButton button)
            {
                // If the current wand state is null, the wand isn't connected.
                // If so, let the application assume the user isn't pressing the button currently.
                var pressed = currentState?.GetButton(button) ?? false;
 
                // If the previous wand state is null, the wand wasn't connected.
                // If so, let the application assume the user wasn't pressing the button last frame.
                var previouslyPressed = previousState?.GetButton(button) ?? false;
 
                // Return true if the button is currently released, but was pressed on the previous frame.
                return previousState.HasValue
                    ? !pressed && previouslyPressed
                    // If the current state exists but the previous state was null, the wand has just connected.
                    // There's no way for the button to be pressed during the previous frame,
                    // so there's no way for the button to have been released this frame. Always return false.
                    : false;
            }
 
            public bool TryGetButtonUp(WandButton button, out bool buttonUp)
            {
                // Even if this operation fails, give buttonUp a default value.
                buttonUp = GetButtonUp(button);
                return currentState.HasValue;
            }
 
            public Vector2 GetStickTilt()
            {
                return currentState?.Stick ?? Vector2.zero;
            }
 
            public bool TryGetStickTilt(out Vector2 stickTilt)
            {
                stickTilt = GetStickTilt();
                return currentState.HasValue;
            }
 
            public float GetTrigger()
            {
                return currentState?.Trigger ?? 0.0f;
            }
 
            public bool TryGetTrigger(out float triggerDisplacement)
            {
                triggerDisplacement = GetTrigger();
                return currentState.HasValue;
            }
 
            #endregion Public Functions
 
 
            #region Overrides
 
            public new void Reset(WandSettings wandSettings)
            {
                base.Reset(wandSettings);
            }
 
            public new virtual void Update(WandSettings wandSettings, ScaleSettings scaleSettings, GameBoardSettings gameBoardSettings)
            {
                if (wandSettings == null)
                {
                    Log.Error("WandSettings configuration required for Wand tracking updates.");
                    return;
                }
 
                base.Update(wandSettings, scaleSettings, gameBoardSettings);
            }
 
            protected override void SetDefaultPoseGameboardSpace(WandSettings settings)
            {
                pose_UGBD = GetDefaultPoseGameboardSpace(settings);
                // We don't have a good offset that we can use for default fingertips/aim poses, so just use the default pose for everything
                fingertipsPose_GameboardSpace = pose_UGBD;
                aimPose_GameboardSpace = pose_UGBD;
            }
 
            protected static Pose GetDefaultPoseGameboardSpace(WandSettings settings)
            {
                Vector3 defaultPosition = DEFAULT_WAND_POSITION_GAME_BOARD_SPACE;
 
                defaultPosition += DEFAULT_WAND_HANDEDNESS_OFFSET_GAME_BOARD_SPACE
                    * (settings.controllerIndex == ControllerIndex.Right ? 1f : -1f);
                return new Pose(defaultPosition, DEFAULT_WAND_ROTATION_GAME_BOARD_SPACE);
            }
 
            protected override void SetPoseUnityWorldSpace(ScaleSettings scaleSettings, GameBoardSettings gameBoardSettings)
            {
                pose_UWRLD = GameboardToWorldSpace(pose_UGBD, scaleSettings, gameBoardSettings);
                fingertipsPose_UnityWorldSpace = GameboardToWorldSpace(fingertipsPose_GameboardSpace, scaleSettings, gameBoardSettings);
                aimPose_UnityWorldSpace = GameboardToWorldSpace(aimPose_GameboardSpace, scaleSettings, gameBoardSettings);
            }
 
            protected override bool TryCheckConnected(out bool connected)
            {
                if (!wandAvailabilityErroredOnce)
                {
                    try
                    {
                        T5_Bool wandAvailable = false;
                        int result = NativePlugin.GetWandAvailability(glassesHandle, ref wandAvailable, controllerIndex);
 
                        if (result == 0)
                        {
                            isConnected = wandAvailable;
                            connected = wandAvailable;
                            return true;
                        }
                    }
                    catch (DllNotFoundException e)
                    {
                        Log.Info("Could not connect to Tilt Five plugin for wand: {0}", e.Message);
                        wandAvailabilityErroredOnce = true;
                    }
                    catch (Exception e)
                    {
                        Log.Error(
                            "Failed to connect to Tilt Five plugin for wand availability: {0}",
                            e.ToString());
                        wandAvailabilityErroredOnce = true;
                    }
                }
 
                isConnected = false;
                connected = false;
                return false;
            }
 
            protected override bool TryGetStateFromPlugin(out T5_ControllerState controllerState, out bool poseIsValid, GameBoardSettings gameBoardSettings)
            {
                if (!TryGetWandControlsState(glassesHandle, out var controllerStateResult, controllerIndex))
                {
                    poseIsValid = false;
                    controllerState = new T5_ControllerState();
 
                    return false;
                }
 
                controllerState = controllerStateResult.Value;
                poseIsValid = Glasses.IsTracked(glassesHandle) && controllerState.PoseValid;
 
                return true;
            }
 
            protected override void SetPoseGameboardSpace(in T5_ControllerState controllerState,
                WandSettings wandSettings, ScaleSettings scaleSettings, GameBoardSettings gameboardSettings)
            {
                // Unity reference frames:
                //
                // UWND        - Unity WaND local space.
                //               +x right, +y up, +z forward
                // UGBD        - Unity Gameboard space.
                //               +x right, +y up, +z forward
                //
                // Tilt Five reference frames:
                //
                // DW          - Our right-handed version of Unity's default wand space.
                //               +x right, +y down, +z forward
                // GBD         - Gameboard space.
                //               +x right, +y forward, +z up
 
                Vector3 gripPosition_UGBD = ConvertPosGBDToUGBD(controllerState.GripPos_GBD);
                Vector3 fingertipsPosition_UGBD = ConvertPosGBDToUGBD(controllerState.FingertipsPos_GBD);
                Vector3 aimPosition_UGBD = ConvertPosGBDToUGBD(controllerState.AimPos_GBD);
                var rotation_UGBD = CalculateRotation(controllerState.RotToWND_GBD);
 
                ProcessTrackingData(gripPosition_UGBD, fingertipsPosition_UGBD, aimPosition_UGBD,
                    rotation_UGBD, wandSettings, scaleSettings, gameboardSettings,
                    out pose_UGBD, out fingertipsPose_GameboardSpace, out aimPose_GameboardSpace);
            }
 
            protected override void SetInvalidPoseGameboardSpace(in T5_ControllerState t5_ControllerState, WandSettings settings, ScaleSettings scaleSettings, GameBoardSettings gameboardSettings)
            {
                SetPoseGameboardSpace(t5_ControllerState, settings, scaleSettings, gameboardSettings);
            }
 
            protected override void SetDrivenObjectTransform(WandSettings wandSettings, ScaleSettings scaleSettings, GameBoardSettings gameBoardSettings)
            {
                if (wandSettings.GripPoint != null)
                {
                    wandSettings.GripPoint.transform.SetPositionAndRotation(gripPose_UnityWorldSpace.position, gripPose_UnityWorldSpace.rotation);
                }
 
                if (wandSettings.FingertipPoint != null)
                {
                    wandSettings.FingertipPoint.transform.SetPositionAndRotation(fingertipsPose_UnityWorldSpace.position, fingertipsPose_UnityWorldSpace.rotation);
                }
 
                if (wandSettings.AimPoint != null)
                {
                    wandSettings.AimPoint.transform.SetPositionAndRotation(aimPose_UnityWorldSpace.position, aimPose_UnityWorldSpace.rotation);
                }
            }
 
            public virtual void Dispose()
            {
                Log.Info($"Glasses {glassesHandle} {controllerIndex} Wand disconnected");
            }
 
            #endregion Overrides
 
 
            #region Private Helper Functions
 
            protected Quaternion CalculateRotation(Quaternion rotToWND_GBD)
            {
                Quaternion rotToDW_GBD = Quaternion.AngleAxis(90f, Vector3.right);
                Quaternion rotToGBD_DW = Quaternion.Inverse(rotToDW_GBD);
                Quaternion rotToWND_DW = rotToWND_GBD * rotToGBD_DW;
                Quaternion rotToUGBD_UWND = new Quaternion(rotToWND_DW.x, -rotToWND_DW.y, rotToWND_DW.z, rotToWND_DW.w);
                return rotToUGBD_UWND;
            }
 
            protected void ProcessTrackingData(Vector3 gripPosition_UGBD, Vector3 fingertipsPosition_UGBD, Vector3 aimPosition_UGBD, Quaternion rotToUGBD_WND,
                WandSettings wandSettings, ScaleSettings scaleSettings, GameBoardSettings gameBoardSettings,
                out Pose gripPose_UGBD, out Pose fingertipsPose_UGBD, out Pose aimPose_UGBD)
            {
                var incomingGripPose_UGBD = new Pose(gripPosition_UGBD, rotToUGBD_WND);
                var incomingFingertipsPose_UGBD = new Pose(fingertipsPosition_UGBD, rotToUGBD_WND);
                var incomingAimPose_UGBD = new Pose(aimPosition_UGBD, rotToUGBD_WND);
 
                // Get the distance between the tracking points and the grip point.
                // Currently, when a pose is considered invalid, the position and rotation reported by the native plugin are completely zero'd out.
                // This crunches the tracking points together unless we consider the stale positions from the previous frame.
                // It also means that we don't get the desired behavior for TrackingFailureMode.FreezePosition,
                // in which the wand position freezes while still showing rotation values reported by the IMU.
                // TODO: In the native plugin, even for invalid poses (both wand and glasses, which are also affected),
                // include an offset for the tracking points, and pass through rotation data.
                var staleGripPose_UGBD = pose_UGBD;
                var staleFingertipsPose_UGBD = fingertipsPose_GameboardSpace;
                var staleAimPose_UGBD = aimPose_GameboardSpace;
 
                var gripPointOffsetDistance = 0f;
                var fingertipsPointOffsetDistance = Mathf.Max((fingertipsPosition_UGBD - gripPosition_UGBD).magnitude,
                    (staleFingertipsPose_UGBD.position - staleGripPose_UGBD.position).magnitude);
                var aimPointOffsetDistance = Mathf.Max((aimPosition_UGBD - gripPosition_UGBD).magnitude,
                    (staleAimPose_UGBD.position - staleGripPose_UGBD.position).magnitude);
 
                // Handle invalid poses
                gripPose_UGBD = FilterTrackingPointPose(staleGripPose_UGBD, staleGripPose_UGBD, incomingGripPose_UGBD, gripPointOffsetDistance, wandSettings);
                fingertipsPose_UGBD = FilterTrackingPointPose(staleGripPose_UGBD, staleFingertipsPose_UGBD, incomingFingertipsPose_UGBD, fingertipsPointOffsetDistance, wandSettings);
                aimPose_UGBD = FilterTrackingPointPose(staleGripPose_UGBD, staleAimPose_UGBD, incomingAimPose_UGBD, aimPointOffsetDistance, wandSettings);
 
            }
 
            protected Pose FilterTrackingPointPose(Pose staleGripPointPose, Pose staleTrackingPointPose,
                Pose newTrackingPointPose, float trackingPointOffsetDistance, WandSettings settings)
            {
                if (!isTracked && settings.RejectUntrackedPositionData)
                {
                    switch (settings.FailureMode)
                    {
                        default:
                        // If we have an undefined FailureMode for some reason, fall through to FreezePosition
                        case TrackableSettings.TrackingFailureMode.FreezePosition:
                            // We need to determine where to put each tracking point.
                            // We only want to freeze the position of the grip point while allowing the other
                            // points to rotate around it, colinear to the grip point's forward vector.
                            var extrapolatedPosition = staleGripPointPose.position +
                                Quaternion.Inverse(newTrackingPointPose.rotation) * Vector3.forward * trackingPointOffsetDistance;
                            return new Pose(extrapolatedPosition, newTrackingPointPose.rotation);
                        case TrackableSettings.TrackingFailureMode.FreezePositionAndRotation:
                            return staleTrackingPointPose;
                        case TrackableSettings.TrackingFailureMode.SnapToDefault:
                            return GetDefaultPoseGameboardSpace(settings);
                    }
                }
                else
                {
                    return newTrackingPointPose;
                }
            }
 
            #endregion Private Helper Functions
        }
 
#if UNITY_2019_1_OR_NEWER && INPUTSYSTEM_AVAILABLE
        private class WandDeviceCore : WandCore
        {
            internal WandDevice wandDevice;
 
            private enum TrackingState : int
            {
                None,
                Limited,
                Tracking
            }
 
            public WandDeviceCore(GlassesHandle glassesHandle, ControllerIndex controllerIndex) : base(glassesHandle, controllerIndex)
            {
                if(Glasses.TryGetGlassesDevice(glassesHandle, out var glassesDevice))
                {
                    wandDevice = Input.GetWandDevice(glassesDevice.PlayerIndex, controllerIndex);
                    InputSystem.QueueConfigChangeEvent(wandDevice);
                    InputSystem.EnableDevice(wandDevice);
 
                    var inputUserCount = InputUser.all.Count;
                    var playerIndex = (int)glassesDevice.PlayerIndex;
                    var headPoseRoot = Glasses.GetPoseRoot(glassesDevice.PlayerIndex);
 
                    if (headPoseRoot != null && inputUserCount >= playerIndex)
                    {
                        var playerInput = headPoseRoot.GetComponentInChildren<PlayerInput>();
 
                        if(playerInput != null)
                        {
                            InputUser.PerformPairingWithDevice(wandDevice, playerInput.user);
                        }
                    }
                }
            }
 
            public override void Dispose()
            {
                base.Dispose();
                InputSystem.QueueConfigChangeEvent(wandDevice);
                InputSystem.DisableDevice(wandDevice);
            }
 
            public override void GetLatestInputs()
            {
                base.GetLatestInputs();
 
                // If the wandDevice isn't added to the input system for any reason,
                // don't bother queueing any delta state events.
                if (!wandDevice.added || !wandDevice.enabled)
                {
                    return;
                }
 
                InputSystem.QueueDeltaStateEvent(wandDevice.TiltFive, currentState.HasValue && currentState.Value.TryGetButton(WandButton.T5));
                InputSystem.QueueDeltaStateEvent(wandDevice.One, currentState.HasValue && currentState.Value.TryGetButton(WandButton.One));
                InputSystem.QueueDeltaStateEvent(wandDevice.Two, currentState.HasValue && currentState.Value.TryGetButton(WandButton.Two));
                InputSystem.QueueDeltaStateEvent(wandDevice.Three, currentState.HasValue && currentState.Value.TryGetButton(WandButton.Three));
                InputSystem.QueueDeltaStateEvent(wandDevice.A, currentState.HasValue && currentState.Value.TryGetButton(WandButton.A));
                InputSystem.QueueDeltaStateEvent(wandDevice.B, currentState.HasValue && currentState.Value.TryGetButton(WandButton.B));
                InputSystem.QueueDeltaStateEvent(wandDevice.X, currentState.HasValue && currentState.Value.TryGetButton(WandButton.X));
                InputSystem.QueueDeltaStateEvent(wandDevice.Y, currentState.HasValue && currentState.Value.TryGetButton(WandButton.Y));
 
                InputSystem.QueueDeltaStateEvent(wandDevice.Stick, currentState.HasValue ? currentState.Value.TryGetStick() : Vector2.zero);
                InputSystem.QueueDeltaStateEvent(wandDevice.Trigger, currentState.HasValue ? currentState.Value.TryGetTrigger() : 0f);
            }
 
            protected override void SetDrivenObjectTransform(WandSettings wandSettings, ScaleSettings scaleSettings, GameBoardSettings gameBoardSettings)
            {
                base.SetDrivenObjectTransform(wandSettings, scaleSettings, gameBoardSettings);
 
                // If the wandDevice isn't added to the input system for any reason,
                // don't bother queueing any delta state events.
                if (!wandDevice.added || !wandDevice.enabled)
                {
                    return;
                }
            
                // Time to inject our wand state into the Input System.
                QueueDeltaStateEvent(wandDevice.devicePosition, pose_UWRLD.position);
                QueueDeltaStateEvent(wandDevice.FingertipsPosition, fingertipsPose_UnityWorldSpace.position);
                QueueDeltaStateEvent(wandDevice.AimPosition, aimPose_UnityWorldSpace.position);
 
                QueueDeltaStateEvent(wandDevice.RawGripPosition, pose_UGBD.position);
                QueueDeltaStateEvent(wandDevice.RawFingertipsPosition, fingertipsPose_GameboardSpace.position);
                QueueDeltaStateEvent(wandDevice.RawAimPosition, aimPose_GameboardSpace.position);
 
                QueueDeltaStateEvent(wandDevice.deviceRotation, pose_UWRLD.rotation);
                QueueDeltaStateEvent(wandDevice.RawRotation, pose_UGBD.rotation);
 
                InputSystem.QueueDeltaStateEvent(wandDevice.isTracked, isTracked);
 
                var trackingState = TrackingState.Tracking;
                if (!isTracked)
                {
                    trackingState = wandSettings.FailureMode == TrackableSettings.TrackingFailureMode.FreezePosition
                        ? TrackingState.Limited
                        : TrackingState.None;
                }
 
                InputSystem.QueueDeltaStateEvent(wandDevice.trackingState, (int)trackingState);
            }
 
            private static void QueueDeltaStateEvent(Vector3Control vector3Control, Vector3 delta)
            {
                InputSystem.QueueDeltaStateEvent(vector3Control.x, delta.x);
                InputSystem.QueueDeltaStateEvent(vector3Control.y, delta.y);
                InputSystem.QueueDeltaStateEvent(vector3Control.z, delta.z);
            }
 
            // For some reason, using QueueDeltaStateEvent on a QuaternionControl with a Quaternion as the delta state doesn't work.
            // As a workaround, let's do it component-wise, since we know floats seem fine.
            private static void QueueDeltaStateEvent(QuaternionControl quaternionControl, Quaternion delta)
            {
                InputSystem.QueueDeltaStateEvent(quaternionControl.w, delta.w);
                InputSystem.QueueDeltaStateEvent(quaternionControl.x, delta.x);
                InputSystem.QueueDeltaStateEvent(quaternionControl.y, delta.y);
                InputSystem.QueueDeltaStateEvent(quaternionControl.z, delta.z);
            }
        }
#endif
 
        #endregion Private Classes
    }
 
}