VRDash/VRDash/Overlay/Config/OverlayLocation/PlayspaceLocation.cs

using Newtonsoft.Json;
using OpenTK;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Valve.VR;
using VRDash.UI.LocationEditors;
using VRDash.Utilities;

namespace VRDash.Overlay.Config.OverlayLocation
{
    [OverlayLocationInfo("Playspace", typeof(PlayspaceLocationEditor))]
    public class PlayspaceLocation : IOverlayLocation
    { 

        [JsonIgnore]
        public Matrix3x4 Location { get; private set; } = new Matrix3x4(new Vector4(1, 0, 0, 0),
                    new Vector4(0, 1, 0, 0),
                    new Vector4(0, 0, 1, 0));

        public float PosX { get; set; } = 0;
        public float PosY { get; set; } = 0;
        public float PosZ { get; set; } = 0;

        public float RotP { get; set; } = 0;
        public float RotY { get; set; } = 0;
        public float RotR { get; set; } = 0;

        public float ScaleX { get; set; } = 1;
        public float ScaleY { get; set; } = 1;
        public float ScaleZ { get; set; } = 1;

        DateTime m_LastCheck = DateTime.MinValue;

        [JsonIgnore]
        public IOverlay Overlay { get; internal set; } = null;

        public OverlayHandles CreateOverlay(string key, string name, IOverlay overlay)
        {
            ulong overlayHandle = 0;
            EVROverlayError err = OpenVR.Overlay.CreateOverlay(key, name, ref overlayHandle);

            if (err != EVROverlayError.None)
            {
                throw new Exception($"Error creating overlay: {err}");
            }

            ETrackedDeviceClass type = OpenVR.System.GetTrackedDeviceClass(0);

            overlay.Update += Overlay_Update;

            Overlay = overlay;
             

            return new OverlayHandles(overlayHandle, 0);
        }

        public void SetLocationBasedOnController()
        {
            uint controllerIndex = OpenVR.System.GetTrackedDeviceIndexForControllerRole(ETrackedControllerRole.LeftHand);

            TrackedDevicePose_t pose = new TrackedDevicePose_t();
            TrackedDevicePose_t gamePose = new TrackedDevicePose_t();
            var err = OpenVR.Compositor.GetLastPoseForTrackedDeviceIndex(controllerIndex, ref pose, ref gamePose);
            if (err != EVRCompositorError.None)
            {
                this.GetLogger(Overlay).Warn($"Unable to set location based on controller: {err}");
                return;
            }

            if (pose.eTrackingResult != ETrackingResult.Running_OK)
            {
                this.GetLogger(Overlay).Warn($"Unable to get reliable tracking: {pose.eTrackingResult}");
            }

            var matrix = gamePose.mDeviceToAbsoluteTracking.ToOpenTKMatrix();

            DecomposeMatrix(matrix);
        }

        private void DecomposeMatrix(Matrix3x4 matrix)
        {
            Vector3 transform = new Vector3(matrix.M14, matrix.M24, matrix.M34);

            PosX = transform.X;
            PosY = transform.Y;
            PosZ = transform.Z;

            Vector3 scaling = new Vector3((float)Math.Sqrt((matrix.M11 * matrix.M11) + (matrix.M12 * matrix.M12) + (matrix.M13 * matrix.M13)),
                (float)Math.Sqrt((matrix.M21 * matrix.M21) + (matrix.M22 * matrix.M22) + (matrix.M23 * matrix.M23)),
                (float)Math.Sqrt((matrix.M31 * matrix.M31) + (matrix.M32 * matrix.M32) + (matrix.M33 * matrix.M33)));

            //ScaleX = scaling.X;
            //ScaleY = scaling.Y;
            //ScaleZ = scaling.Z;

            Matrix3 rotation = new Matrix3()
            {
                M11 = matrix.M11 / scaling.X,
                M12 = matrix.M12 / scaling.X,
                M13 = matrix.M13 / scaling.X,
                M21 = matrix.M21 / scaling.Y,
                M22 = matrix.M22 / scaling.Y,
                M23 = matrix.M23 / scaling.Y,
                M31 = matrix.M31 / scaling.Z,
                M32 = matrix.M32 / scaling.Z,
                M33 = matrix.M33 / scaling.Z
            };

            Quaternion quatRes = Quaternion.FromMatrix(rotation);

            var q = quatRes;

            double pitch = 0;
            double yaw = 0;
            double roll = 0;

            double sinr_cosp = +2.0 * (q.W * q.X + q.Y * q.Z);
            double cosr_cosp = +1.0 - 2.0 * (q.X * q.X + q.Y * q.Y);
            roll = Math.Atan2(sinr_cosp, cosr_cosp);



            // pitch (y-axis rotation)
            double sinp = +2.0 * (q.W * q.Y - q.Z * q.X);
            if (Math.Abs(sinp) >= 1)
                pitch = C.math.copysign(MathHelper.Pi / 2, sinp); // use 90 degrees if out of range
            else
                pitch = Math.Asin(sinp);

            // yaw (z-axis rotation)
            double siny_cosp = +2.0 * (q.W * q.Z + q.X * q.Y);
            double cosy_cosp = +1.0 - 2.0 * (q.Y * q.Y + q.Z * q.Z);
            yaw = Math.Atan2(siny_cosp, cosy_cosp);

            pitch = MathHelper.RadiansToDegrees(pitch);
            yaw = MathHelper.RadiansToDegrees(yaw);
            roll = MathHelper.RadiansToDegrees(roll);

            RotP = (float)pitch;
            RotY = (float)yaw;
            RotR = (float)roll;
        }

        private void Overlay_Update(object sender, EventArgs e)
        {
            if (DateTime.Now.Subtract(m_LastCheck).TotalSeconds >= 1)
            {

                MathHelper.DegreesToRadians(RotP);
                var quat = Quaternion.FromEulerAngles(MathHelper.DegreesToRadians(RotP), MathHelper.DegreesToRadians(RotY), MathHelper.DegreesToRadians(RotR));
                Location = Matrix3x4.Mult(Matrix3x4.CreateTranslation(PosX, PosY, PosZ), Matrix3x4.Mult(Matrix3x4.CreateFromQuaternion(quat), Matrix3x4.CreateScale(ScaleX, ScaleY, ScaleZ)));


                if (Overlay.OverlayHandle == OpenVR.k_ulOverlayHandleInvalid) { return; }

                var matrix = Location.ToOpenVRMatrix();

                var err = OpenVR.Overlay.SetOverlayTransformAbsolute(Overlay.OverlayHandle, ETrackingUniverseOrigin.TrackingUniverseStanding, ref matrix);

                //var err = OpenVR.Overlay.SetOverlayTransformTrackedDeviceRelative(m_Overlay.OverlayHandle, m_ControllerIndex, ref matrix);

                if (err != EVROverlayError.None)
                {
                    this.GetLogger(Overlay).Warn($"Error setting overlay absolute location: {err}");
                }

                m_LastCheck = DateTime.Now;
            }
        }

        public void ResetCheckTimer()
        {
            m_LastCheck = DateTime.MinValue;
        }
    }
}