SteamVR_Utils.cs

//======= Copyright (c) Valve Corporation, All rights reserved. ===============
//
// Purpose: Utilities for working with SteamVR
//
//=============================================================================

using UnityEngine;
using System.Collections;
using System.Runtime.InteropServices;
using Valve.VR;

public static class SteamVR_Utils
{
    // this version does not clamp [0..1]
    public static Quaternion Slerp(Quaternion A, Quaternion B, float t)
    {
        var cosom = Mathf.Clamp(A.x * B.x + A.y * B.y + A.z * B.z + A.w * B.w, -1.0f, 1.0f);
        if (cosom < 0.0f)
        {
            B = new Quaternion(-B.x, -B.y, -B.z, -B.w);
            cosom = -cosom;
        }

        float sclp, sclq;
        if ((1.0f - cosom) > 0.0001f)
        {
            var omega = Mathf.Acos(cosom);
            var sinom = Mathf.Sin(omega);
            sclp = Mathf.Sin((1.0f - t) * omega) / sinom;
            sclq = Mathf.Sin(t * omega) / sinom;
        }
        else
        {
            // "from" and "to" very close, so do linear interp
            sclp = 1.0f - t;
            sclq = t;
        }

        return new Quaternion(
            sclp * A.x + sclq * B.x,
            sclp * A.y + sclq * B.y,
            sclp * A.z + sclq * B.z,
            sclp * A.w + sclq * B.w);
    }

    public static Vector3 Lerp(Vector3 A, Vector3 B, float t)
    {
        return new Vector3(
            Lerp(A.x, B.x, t),
            Lerp(A.y, B.y, t),
            Lerp(A.z, B.z, t));
    }

    public static float Lerp(float A, float B, float t)
    {
        return A + (B - A) * t;
    }

    public static double Lerp(double A, double B, double t)
    {
        return A + (B - A) * t;
    }

    public static float InverseLerp(Vector3 A, Vector3 B, Vector3 result)
    {
        return Vector3.Dot(result - A, B - A);
    }

    public static float InverseLerp(float A, float B, float result)
    {
        return (result - A) / (B - A);
    }

    public static double InverseLerp(double A, double B, double result)
    {
        return (result - A) / (B - A);
    }

    public static float Saturate(float A)
    {
        return (A < 0) ? 0 : (A > 1) ? 1 : A;
    }

    public static Vector2 Saturate(Vector2 A)
    {
        return new Vector2(Saturate(A.x), Saturate(A.y));
    }

    public static float Abs(float A)
    {
        return (A < 0) ? -A : A;
    }

    public static Vector2 Abs(Vector2 A)
    {
        return new Vector2(Abs(A.x), Abs(A.y));
    }

    private static float _copysign(float sizeval, float signval)
    {
        return Mathf.Sign(signval) == 1 ? Mathf.Abs(sizeval) : -Mathf.Abs(sizeval);
    }

    public static Quaternion GetRotation(this Matrix4x4 matrix)
    {
        Quaternion q = new Quaternion();
        q.w = Mathf.Sqrt(Mathf.Max(0, 1 + matrix.m00 + matrix.m11 + matrix.m22)) / 2;
        q.x = Mathf.Sqrt(Mathf.Max(0, 1 + matrix.m00 - matrix.m11 - matrix.m22)) / 2;
        q.y = Mathf.Sqrt(Mathf.Max(0, 1 - matrix.m00 + matrix.m11 - matrix.m22)) / 2;
        q.z = Mathf.Sqrt(Mathf.Max(0, 1 - matrix.m00 - matrix.m11 + matrix.m22)) / 2;
        q.x = _copysign(q.x, matrix.m21 - matrix.m12);
        q.y = _copysign(q.y, matrix.m02 - matrix.m20);
        q.z = _copysign(q.z, matrix.m10 - matrix.m01);
        return q;
    }

    public static Vector3 GetPosition(this Matrix4x4 matrix)
    {
        var x = matrix.m03;
        var y = matrix.m13;
        var z = matrix.m23;

        return new Vector3(x, y, z);
    }

    public static Vector3 GetScale(this Matrix4x4 m)
    {
        var x = Mathf.Sqrt(m.m00 * m.m00 + m.m01 * m.m01 + m.m02 * m.m02);
        var y = Mathf.Sqrt(m.m10 * m.m10 + m.m11 * m.m11 + m.m12 * m.m12);
        var z = Mathf.Sqrt(m.m20 * m.m20 + m.m21 * m.m21 + m.m22 * m.m22);

        return new Vector3(x, y, z);
    }

    [System.Serializable]
    public struct RigidTransform
    {
        public Vector3 pos;
        public Quaternion rot;

        public static RigidTransform identity
        {
            get { return new RigidTransform(Vector3.zero, Quaternion.identity); }
        }

        public static RigidTransform FromLocal(Transform t)
        {
            return new RigidTransform(t.localPosition, t.localRotation);
        }

        public RigidTransform(Vector3 pos, Quaternion rot)
        {
            this.pos = pos;
            this.rot = rot;
        }

        public RigidTransform(Transform t)
        {
            this.pos = t.position;
            this.rot = t.rotation;
        }

        public RigidTransform(Transform from, Transform to)
        {
            var inv = Quaternion.Inverse(from.rotation);
            rot = inv * to.rotation;
            pos = inv * (to.position - from.position);
        }

        public RigidTransform(HmdMatrix34_t pose)
        {
            var m = Matrix4x4.identity;

            m[0, 0] =  pose.m0;
            m[0, 1] =  pose.m1;
            m[0, 2] = -pose.m2;
            m[0, 3] =  pose.m3;

            m[1, 0] =  pose.m4;
            m[1, 1] =  pose.m5;
            m[1, 2] = -pose.m6;
            m[1, 3] =  pose.m7;

            m[2, 0] = -pose.m8;
            m[2, 1] = -pose.m9;
            m[2, 2] =  pose.m10;
            m[2, 3] = -pose.m11;

            this.pos = m.GetPosition();
            this.rot = m.GetRotation();
        }

        public RigidTransform(HmdMatrix44_t pose)
        {
            var m = Matrix4x4.identity;

            m[0, 0] =  pose.m0;
            m[0, 1] =  pose.m1;
            m[0, 2] = -pose.m2;
            m[0, 3] =  pose.m3;

            m[1, 0] =  pose.m4;
            m[1, 1] =  pose.m5;
            m[1, 2] = -pose.m6;
            m[1, 3] =  pose.m7;

            m[2, 0] = -pose.m8;
            m[2, 1] = -pose.m9;
            m[2, 2] =  pose.m10;
            m[2, 3] = -pose.m11;

            m[3, 0] =  pose.m12;
            m[3, 1] =  pose.m13;
            m[3, 2] = -pose.m14;
            m[3, 3] =  pose.m15;

            this.pos = m.GetPosition();
            this.rot = m.GetRotation();
        }

        public HmdMatrix44_t ToHmdMatrix44()
        {
            var m = Matrix4x4.TRS(pos, rot, Vector3.one);
            var pose = new HmdMatrix44_t();

            pose.m0  =  m[0, 0];
            pose.m1  =  m[0, 1];
            pose.m2  = -m[0, 2];
            pose.m3  =  m[0, 3];

            pose.m4  =  m[1, 0];
            pose.m5  =  m[1, 1];
            pose.m6  = -m[1, 2];
            pose.m7  =  m[1, 3];

            pose.m8  = -m[2, 0];
            pose.m9  = -m[2, 1];
            pose.m10 =  m[2, 2];
            pose.m11 = -m[2, 3];

            pose.m12 =  m[3, 0];
            pose.m13 =  m[3, 1];
            pose.m14 = -m[3, 2];
            pose.m15 =  m[3, 3];

            return pose;
        }

        public HmdMatrix34_t ToHmdMatrix34()
        {
            var m = Matrix4x4.TRS(pos, rot, Vector3.one);
            var pose = new HmdMatrix34_t();

            pose.m0  =  m[0, 0];
            pose.m1  =  m[0, 1];
            pose.m2  = -m[0, 2];
            pose.m3  =  m[0, 3];

            pose.m4  =  m[1, 0];
            pose.m5  =  m[1, 1];
            pose.m6  = -m[1, 2];
            pose.m7  =  m[1, 3];

            pose.m8  = -m[2, 0];
            pose.m9  = -m[2, 1];
            pose.m10 =  m[2, 2];
            pose.m11 = -m[2, 3];

            return pose;
        }

        public override bool Equals(object o)
        {
            if (o is RigidTransform)
            {
                RigidTransform t = (RigidTransform)o;
                return pos == t.pos && rot == t.rot;
            }
            return false;
        }

        public override int GetHashCode()
        {
            return pos.GetHashCode() ^ rot.GetHashCode();
        }

        public static bool operator ==(RigidTransform a, RigidTransform b)
        {
            return a.pos == b.pos && a.rot == b.rot;
        }

        public static bool operator !=(RigidTransform a, RigidTransform b)
        {
            return a.pos != b.pos || a.rot != b.rot;
        }

        public static RigidTransform operator *(RigidTransform a, RigidTransform b)
        {
            return new RigidTransform
            {
                rot = a.rot * b.rot,
                pos = a.pos + a.rot * b.pos
            };
        }

        public void Inverse()
        {
            rot = Quaternion.Inverse(rot);
            pos = -(rot * pos);
        }

        public RigidTransform GetInverse()
        {
            var t = new RigidTransform(pos, rot);
            t.Inverse();
            return t;
        }

        public void Multiply(RigidTransform a, RigidTransform b)
        {
            rot = a.rot * b.rot;
            pos = a.pos + a.rot * b.pos;
        }

        public Vector3 InverseTransformPoint(Vector3 point)
        {
            return Quaternion.Inverse(rot) * (point - pos);
        }

        public Vector3 TransformPoint(Vector3 point)
        {
            return pos + (rot * point);
        }

        public static Vector3 operator *(RigidTransform t, Vector3 v)
        {
            return t.TransformPoint(v);
        }

        public static RigidTransform Interpolate(RigidTransform a, RigidTransform b, float t)
        {
            return new RigidTransform(Vector3.Lerp(a.pos, b.pos, t), Quaternion.Slerp(a.rot, b.rot, t));
        }

        public void Interpolate(RigidTransform to, float t)
        {
            pos = SteamVR_Utils.Lerp(pos, to.pos, t);
            rot = SteamVR_Utils.Slerp(rot, to.rot, t);
        }
    }

    public delegate object SystemFn(CVRSystem system, params object[] args);

    public static object CallSystemFn(SystemFn fn, params object[] args)
    {
        var initOpenVR = (!SteamVR.active && !SteamVR.usingNativeSupport);
        if (initOpenVR)
        {
            var error = EVRInitError.None;
            OpenVR.Init(ref error, EVRApplicationType.VRApplication_Utility);
        }

        var system = OpenVR.System;
        var result = (system != null) ? fn(system, args) : null;

        if (initOpenVR)
            OpenVR.Shutdown();

        return result;
    }

    public static void TakeStereoScreenshot(uint screenshotHandle, GameObject target, int cellSize, float ipd, ref string previewFilename, ref string VRFilename)
    {
        const int width = 4096;
        const int height = width / 2;
        const int halfHeight = height / 2;

        var texture = new Texture2D(width, height * 2, TextureFormat.ARGB32, false);

        var timer = new System.Diagnostics.Stopwatch();

        Camera tempCamera = null;

        timer.Start();

        var camera = target.GetComponent<Camera>();
        if (camera == null)
        {
            if (tempCamera == null)
                tempCamera = new GameObject().AddComponent<Camera>();
            camera = tempCamera;
        }

        // Render preview texture
        const int previewWidth = 2048;
        const int previewHeight = 2048;
        var previewTexture = new Texture2D(previewWidth, previewHeight, TextureFormat.ARGB32, false);
        var targetPreviewTexture = new RenderTexture(previewWidth, previewHeight, 24);

        var oldTargetTexture = camera.targetTexture;
        var oldOrthographic = camera.orthographic;
        var oldFieldOfView = camera.fieldOfView;
        var oldAspect = camera.aspect;
        var oldstereoTargetEye = camera.stereoTargetEye;
        camera.stereoTargetEye = StereoTargetEyeMask.None;
        camera.fieldOfView = 60.0f;
        camera.orthographic = false;
        camera.targetTexture = targetPreviewTexture;
        camera.aspect = 1.0f;
        camera.Render();

        // copy preview texture
        RenderTexture.active = targetPreviewTexture;
        previewTexture.ReadPixels(new Rect(0, 0, targetPreviewTexture.width, targetPreviewTexture.height), 0, 0);
        RenderTexture.active = null;
        camera.targetTexture = null;
        Object.DestroyImmediate(targetPreviewTexture);

        var fx = camera.gameObject.AddComponent<SteamVR_SphericalProjection>();

        var oldPosition = target.transform.localPosition;
        var oldRotation = target.transform.localRotation;
        var basePosition = target.transform.position;
        var baseRotation = Quaternion.Euler(0, target.transform.rotation.eulerAngles.y, 0);

        var transform = camera.transform;

        int vTotal = halfHeight / cellSize;
        float dv = 90.0f / vTotal; // vertical degrees per segment
        float dvHalf = dv / 2.0f;

        var targetTexture = new RenderTexture(cellSize, cellSize, 24);
        targetTexture.wrapMode = TextureWrapMode.Clamp;
        targetTexture.antiAliasing = 8;

        camera.fieldOfView = dv;
        camera.orthographic = false;
        camera.targetTexture = targetTexture;
        camera.aspect = oldAspect;
        camera.stereoTargetEye = StereoTargetEyeMask.None;

        // Render sections of a sphere using a rectilinear projection
        // and resample using a sphereical projection into a single panorama
        // texture per eye.  We break into sections in order to keep the eye
        // separation similar around the sphere.  Rendering alternates between
        // top and bottom sections, sweeping horizontally around the sphere,
        // alternating left and right eyes.
        for (int v = 0; v < vTotal; v++)
        {
            var pitch = 90.0f - (v * dv) - dvHalf;
            var uTotal = width / targetTexture.width;
            var du = 360.0f / uTotal; // horizontal degrees per segment
            var duHalf = du / 2.0f;

            var vTarget = v * halfHeight / vTotal;

            for (int i = 0; i < 2; i++) // top, bottom
            {
                if (i == 1)
                {
                    pitch = -pitch;
                    vTarget = height - vTarget - cellSize;
                }

                for (int u = 0; u < uTotal; u++)
                {
                    var yaw = -180.0f + (u * du) + duHalf;

                    var uTarget = u * width / uTotal;

                    var vTargetOffset = 0;
                    var xOffset = -ipd / 2 * Mathf.Cos(pitch * Mathf.Deg2Rad);

                    for (int j = 0; j < 2; j++) // left, right
                    {
                        if (j == 1)
                        {
                            vTargetOffset = height;
                            xOffset = -xOffset;
                        }

                        var offset = baseRotation * Quaternion.Euler(0, yaw, 0) * new Vector3(xOffset, 0, 0);
                        transform.position = basePosition + offset;

                        var direction = Quaternion.Euler(pitch, yaw, 0.0f);
                        transform.rotation = baseRotation * direction;

                        // vector pointing to center of this section
                        var N = direction * Vector3.forward;

                        // horizontal span of this section in degrees
                        var phi0 = yaw - (du / 2);
                        var phi1 = phi0 + du;

                        // vertical span of this section in degrees
                        var theta0 = pitch + (dv / 2);
                        var theta1 = theta0 - dv;

                        var midPhi = (phi0 + phi1) / 2;
                        var baseTheta = Mathf.Abs(theta0) < Mathf.Abs(theta1) ? theta0 : theta1;

                        // vectors pointing to corners of image closes to the equator
                        var V00 = Quaternion.Euler(baseTheta, phi0, 0.0f) * Vector3.forward;
                        var V01 = Quaternion.Euler(baseTheta, phi1, 0.0f) * Vector3.forward;

                        // vectors pointing to top and bottom midsection of image
                        var V0M = Quaternion.Euler(theta0, midPhi, 0.0f) * Vector3.forward;
                        var V1M = Quaternion.Euler(theta1, midPhi, 0.0f) * Vector3.forward;

                        // intersection points for each of the above
                        var P00 = V00 / Vector3.Dot(V00, N);
                        var P01 = V01 / Vector3.Dot(V01, N);
                        var P0M = V0M / Vector3.Dot(V0M, N);
                        var P1M = V1M / Vector3.Dot(V1M, N);

                        // calculate basis vectors for plane
                        var P00_P01 = P01 - P00;
                        var P0M_P1M = P1M - P0M;

                        var uMag = P00_P01.magnitude;
                        var vMag = P0M_P1M.magnitude;

                        var uScale = 1.0f / uMag;
                        var vScale = 1.0f / vMag;

                        var uAxis = P00_P01 * uScale;
                        var vAxis = P0M_P1M * vScale;

                        // update material constant buffer
                        fx.Set(N, phi0, phi1, theta0, theta1,
                            uAxis, P00, uScale,
                            vAxis, P0M, vScale);

                        camera.aspect = uMag / vMag;
                        camera.Render();

                        RenderTexture.active = targetTexture;
                        texture.ReadPixels(new Rect(0, 0, targetTexture.width, targetTexture.height), uTarget, vTarget + vTargetOffset);
                        RenderTexture.active = null;                 
                    }

                    // Update progress
                    var progress = (float)( v * ( uTotal * 2.0f ) + u + i*uTotal) / (float)(vTotal * ( uTotal * 2.0f ) );
                    OpenVR.Screenshots.UpdateScreenshotProgress(screenshotHandle, progress);
                }
            }
        }

        // 100% flush
        OpenVR.Screenshots.UpdateScreenshotProgress(screenshotHandle, 1.0f);

        // Save textures to disk.
        // Add extensions
        previewFilename += ".png";
        VRFilename += ".png";

        // Preview
        previewTexture.Apply();
        System.IO.File.WriteAllBytes(previewFilename, previewTexture.EncodeToPNG());

        // VR
        texture.Apply();
        System.IO.File.WriteAllBytes(VRFilename, texture.EncodeToPNG());

        // Cleanup.
        if (camera != tempCamera)
        {
            camera.targetTexture = oldTargetTexture;
            camera.orthographic = oldOrthographic;
            camera.fieldOfView = oldFieldOfView;
            camera.aspect = oldAspect;
            camera.stereoTargetEye = oldstereoTargetEye;

            target.transform.localPosition = oldPosition;
            target.transform.localRotation = oldRotation;
        }
        else
        {
            tempCamera.targetTexture = null;
        }

        Object.DestroyImmediate(targetTexture);
        Object.DestroyImmediate(fx);

        timer.Stop();
        Debug.Log(string.Format("Screenshot took {0} seconds.", timer.Elapsed));

        if (tempCamera != null)
        {
            Object.DestroyImmediate(tempCamera.gameObject);
        }

        Object.DestroyImmediate(previewTexture);
        Object.DestroyImmediate(texture);
    }
}