unity_package/Assets/SteamVR/Scripts/SteamVR_TrackedCamera.cs

//======= Copyright (c) Valve Corporation, All rights reserved. ===============
//
// Purpose: Provides access to video feed and poses of tracked cameras.
//
// Usage:
//			var source = SteamVR_TrackedCamera.Distorted();
//			var source = SteamVR_TrackedCamera.Undistorted();
// or
//			var undistorted = true; // or false
//			var source = SteamVR_TrackedCamera.Source(undistorted);
//
// - Distorted feeds are the decoded images from the camera.
// - Undistorted feeds correct for the camera lens distortion (a.k.a. fisheye)
//   to make straight lines straight.
//
// VideoStreamTexture objects must be symmetrically Acquired and Released to
// ensure the video stream is activated, and shutdown properly once there are
// no more consumers.  You only need to Acquire once when starting to use a
// stream, and Release when you are done using it (as opposed to every frame).
//
//=============================================================================

using UnityEngine;
using Valve.VR;

public class SteamVR_TrackedCamera
{
	public class VideoStreamTexture
	{
		public VideoStreamTexture(uint deviceIndex, bool undistorted)
		{
			this.undistorted = undistorted;
			videostream = Stream(deviceIndex);
		}
		public bool undistorted { get; private set; }
		public uint deviceIndex { get { return videostream.deviceIndex; } }
		public bool hasCamera { get { return videostream.hasCamera; } }
		public bool hasTracking { get { Update(); return header.standingTrackedDevicePose.bPoseIsValid; } }
		public uint frameId { get { Update(); return header.nFrameSequence; } }
		public VRTextureBounds_t frameBounds { get; private set; }
		public EVRTrackedCameraFrameType frameType { get { return undistorted ? EVRTrackedCameraFrameType.Undistorted : EVRTrackedCameraFrameType.Distorted; } }

		Texture2D _texture;
		public Texture2D texture { get { Update(); return _texture; } }

		public SteamVR_Utils.RigidTransform transform { get { Update(); return new SteamVR_Utils.RigidTransform(header.standingTrackedDevicePose.mDeviceToAbsoluteTracking); } }
		public Vector3 velocity { get { Update(); var pose = header.standingTrackedDevicePose; return new Vector3(pose.vVelocity.v0, pose.vVelocity.v1, -pose.vVelocity.v2); } }
		public Vector3 angularVelocity { get { Update(); var pose = header.standingTrackedDevicePose; return new Vector3(-pose.vAngularVelocity.v0, -pose.vAngularVelocity.v1, pose.vAngularVelocity.v2); } }

		public TrackedDevicePose_t GetPose() { Update(); return header.standingTrackedDevicePose; }

		public ulong Acquire()
		{
			return videostream.Acquire();
		}
		public ulong Release()
		{
			var result = videostream.Release();

			if (videostream.handle == 0)
			{
				Object.Destroy(_texture);
				_texture = null;
			}

			return result;
		}

		int prevFrameCount = -1;
		void Update()
		{
			if (Time.frameCount == prevFrameCount)
				return;

			prevFrameCount = Time.frameCount;

			if (videostream.handle == 0)
				return;

			var vr = SteamVR.instance;
			if (vr == null)
				return;

			var trackedCamera = OpenVR.TrackedCamera;
			if (trackedCamera == null)
				return;

			var nativeTex = System.IntPtr.Zero;
			var deviceTexture = (_texture != null) ? _texture : new Texture2D(2, 2);
			var headerSize = (uint)System.Runtime.InteropServices.Marshal.SizeOf(header.GetType());

			if (vr.graphicsAPI == EGraphicsAPIConvention.API_OpenGL)
			{
				if (glTextureId != 0)
					trackedCamera.ReleaseVideoStreamTextureGL(videostream.handle, glTextureId);

				if (trackedCamera.GetVideoStreamTextureGL(videostream.handle, frameType, ref glTextureId, ref header, headerSize) != EVRTrackedCameraError.None)
					return;

				nativeTex = (System.IntPtr)glTextureId;
            }
			else
			{
				if (trackedCamera.GetVideoStreamTextureD3D11(videostream.handle, frameType, deviceTexture.GetNativeTexturePtr(), ref nativeTex, ref header, headerSize) != EVRTrackedCameraError.None)
					return;
			}

			if (_texture == null)
			{
				_texture = Texture2D.CreateExternalTexture((int)header.nWidth, (int)header.nHeight, TextureFormat.RGBA32, false, false, nativeTex);

				uint width = 0, height = 0;
				var frameBounds = new VRTextureBounds_t();
				if (trackedCamera.GetVideoStreamTextureSize(deviceIndex, frameType, ref frameBounds, ref width, ref height) == EVRTrackedCameraError.None)
				{
					// Account for textures being upside-down in Unity.
					frameBounds.vMin = 1.0f - frameBounds.vMin;
					frameBounds.vMax = 1.0f - frameBounds.vMax;
					this.frameBounds = frameBounds;
				}
			}
			else
			{
				_texture.UpdateExternalTexture(nativeTex);
			}
		}

		uint glTextureId;
		VideoStream videostream;
		CameraVideoStreamFrameHeader_t header;
	}

	#region Top level accessors.

	public static VideoStreamTexture Distorted(int deviceIndex = (int)OpenVR.k_unTrackedDeviceIndex_Hmd)
	{
		if (distorted == null)
			distorted = new VideoStreamTexture[OpenVR.k_unMaxTrackedDeviceCount];
		if (distorted[deviceIndex] == null)
			distorted[deviceIndex] = new VideoStreamTexture((uint)deviceIndex, false);
		return distorted[deviceIndex];
	}

	public static VideoStreamTexture Undistorted(int deviceIndex = (int)OpenVR.k_unTrackedDeviceIndex_Hmd)
	{
		if (undistorted == null)
			undistorted = new VideoStreamTexture[OpenVR.k_unMaxTrackedDeviceCount];
		if (undistorted[deviceIndex] == null)
			undistorted[deviceIndex] = new VideoStreamTexture((uint)deviceIndex, true);
		return undistorted[deviceIndex];
	}

	public static VideoStreamTexture Source(bool undistorted, int deviceIndex = (int)OpenVR.k_unTrackedDeviceIndex_Hmd)
	{
		return undistorted ? Undistorted(deviceIndex) : Distorted(deviceIndex);
    }

	private static VideoStreamTexture[] distorted, undistorted;

	#endregion

	#region Internal class to manage lifetime of video streams (per device).

	class VideoStream
	{
		public VideoStream(uint deviceIndex)
		{
			this.deviceIndex = deviceIndex;
			var trackedCamera = OpenVR.TrackedCamera;
			if (trackedCamera != null)
				trackedCamera.HasCamera(deviceIndex, ref _hasCamera);
		}
		public uint deviceIndex { get; private set; }

		ulong _handle;
		public ulong handle { get { return _handle; } }

		bool _hasCamera;
		public bool hasCamera { get { return _hasCamera; } }

		ulong refCount;
		public ulong Acquire()
		{
			if (_handle == 0 && hasCamera)
			{
				var trackedCamera = OpenVR.TrackedCamera;
				if (trackedCamera != null)
					trackedCamera.AcquireVideoStreamingService(deviceIndex, ref _handle);
			}
			return ++refCount;
		}
		public ulong Release()
		{
			if (refCount > 0 && --refCount == 0 && _handle != 0)
			{
				var trackedCamera = OpenVR.TrackedCamera;
				if (trackedCamera != null)
					trackedCamera.ReleaseVideoStreamingService(_handle);
				_handle = 0;
			}
			return refCount;
		}
	}

	static VideoStream Stream(uint deviceIndex)
	{
		if (videostreams == null)
			videostreams = new VideoStream[OpenVR.k_unMaxTrackedDeviceCount];
		if (videostreams[deviceIndex] == null)
			videostreams[deviceIndex] = new VideoStream(deviceIndex);
		return videostreams[deviceIndex];
	}

	static VideoStream[] videostreams;

	#endregion
}