WalkDetectorVR

WalkDetectorVR.cs

/* 

Copyright 20XX, sysdia.com & GPepos

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated 
documentation files (the "Software"), to deal in the Software without restriction, including without 
limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 
copies of the Software, and to permit persons to whom the Software is furnished to do so, 
subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies 
or substantialportions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR 
A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, 
TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 
OTHER DEALINGS IN THE SOFTWARE.
*/

using System.Collections;
using UnityEngine;
using UnityEngine.Events;

namespace Storyclock // https://www.storyclocklabs.com (© 2022 Geoffrey Pepos)
{
    // Oculus, Meta, Quest, Pico, Rift, Vive, VR, Walk/Run Detection

    /* A simple platform agnostic class to measure head and hand movement to drive a Unity VR player.
     * Attach to any gameobject in scene, a master controller gameobject, or the player.
     * Note: can also be used for "flying" with arm flapping.
     */

    public class WalkDetectorVR : MonoBehaviour
    {
        //Version 1.01, 10/10/22

        [Header("For Oculus/Meta, normally Center Eye Anchor, on other platforms could be, head or whatever.")]
        public GameObject playerHead;
        public bool enableHeadMovementDetection = true;

        [Header("How much head movement triggers a step. Recommend 4 to start.")]
        public float headSensitivity = 4f;

        [Header("Hand GameObjects")]
        public GameObject leftHand;
        public bool enableLeftHandMovementDetection = true;
        [Header("")]
        public GameObject rightHand;
        public bool enableRightHandMovementDetection = true;

        [Header("How much hand movement triggers a step. Recommend 7 to start.")]
        public float handSensitivity = 7f;
        [Header("Which hand axis has an effect.")]
        public bool x = true;
        public bool y = true;
        public bool z = true;

        [Header("Set this to a method in your player's movement class. Passes a relative max velocity of head and/or hands.")]
        public UnityEvent<float> OnStepDetected;

        private VelocityEstimator veHead, veHandL, veHandR;

        private void Start()
        {
            veHead = playerHead.AddComponent<VelocityEstimator>();
            veHandR = rightHand.AddComponent<VelocityEstimator>();
            veHandL = leftHand.AddComponent<VelocityEstimator>();
        }

        private void Update()
        {
            float headVelocity, handVelocityR, handVelocityL, maxVelocity;
            Vector3 handVelocityVL, handVelocityVR;

            headVelocity = Mathf.Abs(veHead.GetAccelerationEstimate().y);
            handVelocityVL = veHandL.GetAccelerationEstimate();
            handVelocityVR = veHandR.GetAccelerationEstimate();

            if (!x)
                handVelocityVL.x = handVelocityVR.x = 0;
            if (!y)
                handVelocityVL.y = handVelocityVR.y = 0;
            if (!z)
                handVelocityVL.z = handVelocityVR.z = 0;

            handVelocityL = handVelocityVL.magnitude;
            handVelocityR = handVelocityVR.magnitude;

            if ((enableHeadMovementDetection && headVelocity > headSensitivity)
                || (enableLeftHandMovementDetection && handVelocityL > handSensitivity)
                || (enableRightHandMovementDetection && handVelocityR > handSensitivity))
            {
                maxVelocity = headVelocity;
                if (handVelocityL > maxVelocity)
                    maxVelocity = handVelocityL;
                if (handVelocityR > maxVelocity)
                    maxVelocity = handVelocityR;

                if (OnStepDetected != null)
                    OnStepDetected.Invoke(maxVelocity);
            }
        }
    }

    public class VelocityEstimator : MonoBehaviour
    {
        /* This class is from the free VRTK Toolkit V3 on the unity Asset store.
        * V3 has been depreciated.
        * https://assetstore.unity.com/packages/tools/integration/vrtk-virtual-reality-toolkit-vr-toolkit-64131
        * 
        * Version 4
        * https://assetstore.unity.com/packages/tools/utilities/vrtk-v4-tilia-package-importer-214936
        * 
        * Copyright Website:
        * https://www.sysdia.com
        */

        [Header("Begin the sampling routine when the script is enabled.")]
        public bool autoStartSampling = true;
        [Header("The number of frames to average when calculating velocity.")]
        public int velocityAverageFrames = 5;
        [Header("The number of frames to average when calculating angular velocity.")]
        public int angularVelocityAverageFrames = 10;

        protected Vector3[] velocitySamples;
        protected Vector3[] angularVelocitySamples;
        protected int currentSampleCount;
        protected Coroutine calculateSamplesRoutine;

        public virtual void StartEstimation()
        {
            EndEstimation();
            calculateSamplesRoutine = StartCoroutine(EstimateVelocity());
        }

        public virtual void EndEstimation()
        {
            if (calculateSamplesRoutine != null)
            {
                StopCoroutine(calculateSamplesRoutine);
                calculateSamplesRoutine = null;
            }
        }

        public virtual Vector3 GetVelocityEstimate()
        {
            Vector3 velocity = Vector3.zero;
            int velocitySampleCount = Mathf.Min(currentSampleCount, velocitySamples.Length);
            if (velocitySampleCount != 0)
            {
                for (int i = 0; i < velocitySampleCount; i++)
                {
                    velocity += velocitySamples[i];
                }
                velocity *= (1.0f / velocitySampleCount);
            }
            return velocity;
        }

        public virtual Vector3 GetAngularVelocityEstimate()
        {
            Vector3 angularVelocity = Vector3.zero;
            int angularVelocitySampleCount = Mathf.Min(currentSampleCount, angularVelocitySamples.Length);
            if (angularVelocitySampleCount != 0)
            {
                for (int i = 0; i < angularVelocitySampleCount; i++)
                {
                    angularVelocity += angularVelocitySamples[i];
                }
                angularVelocity *= (1.0f / angularVelocitySampleCount);
            }

            return angularVelocity;
        }

        public virtual Vector3 GetAccelerationEstimate()
        {
            Vector3 average = Vector3.zero;
            for (int i = 2 + currentSampleCount - velocitySamples.Length; i < currentSampleCount; i++)
            {
                if (i < 2)
                {
                    continue;
                }

                int first = i - 2;
                int second = i - 1;

                Vector3 v1 = velocitySamples[first % velocitySamples.Length];
                Vector3 v2 = velocitySamples[second % velocitySamples.Length];
                average += v2 - v1;
            }
            average *= (1.0f / Time.deltaTime);
            return average;
        }

        protected virtual void OnEnable()
        {
            InitArrays();
            if (autoStartSampling)
            {
                StartEstimation();
            }
        }

        protected virtual void OnDisable()
        {
            EndEstimation();
        }

        protected virtual void InitArrays()
        {
            velocitySamples = new Vector3[velocityAverageFrames];
            angularVelocitySamples = new Vector3[angularVelocityAverageFrames];
        }

        protected virtual IEnumerator EstimateVelocity()
        {
            currentSampleCount = 0;

            Vector3 previousPosition = transform.localPosition;
            Quaternion previousRotation = transform.localRotation;
            while (true)
            {
                yield return new WaitForEndOfFrame();

                float velocityFactor = 1.0f / Time.deltaTime;

                int v = currentSampleCount % velocitySamples.Length;
                int w = currentSampleCount % angularVelocitySamples.Length;
                currentSampleCount++;

                velocitySamples[v] = velocityFactor * (transform.localPosition - previousPosition);
                Quaternion deltaRotation = transform.localRotation * Quaternion.Inverse(previousRotation);

                float theta = 2.0f * Mathf.Acos(Mathf.Clamp(deltaRotation.w, -1.0f, 1.0f));
                if (theta > Mathf.PI)
                {
                    theta -= 2.0f * Mathf.PI;
                }

                Vector3 angularVelocity = new Vector3(deltaRotation.x, deltaRotation.y, deltaRotation.z);
                if (angularVelocity.sqrMagnitude > 0.0f)
                {
                    angularVelocity = theta * velocityFactor * angularVelocity.normalized;
                }

                angularVelocitySamples[w] = angularVelocity;

                previousPosition = transform.localPosition;
                previousRotation = transform.localRotation;
            }
        }
    }
}