StereoKit Occlusion Mesh

OcclusionMesh.cs

#if WINDOWS_UWP

using StereoKit;
using System;
using System.Collections.Generic;
using System.Numerics;
using Windows.Foundation;
using Windows.Perception.Spatial;
using Windows.Perception.Spatial.Surfaces;
using Windows.Storage.Streams;

namespace StereoKit
{
	/// <summary>This Requires the Spatial Perception permission in order to
	/// work properly! This contains a hack that makes it a placeholder solution
	/// only. The mesh will ONLY line up if the user remains entirely motionless
	/// during startup.</summary>
	public class OcclusionMesh
	{
		class SurfaceMesh
		{
			public Mesh           mesh;
			public DateTimeOffset lastUpdate;
			public Matrix         at;
			public Guid           id;
		}

		// A shader that does only the basics, outputs a solid color
		const string shader = @"
#include <stereokit>

cbuffer ParamBuffer : register(b2) {
	// [param] color color {1, 1, 1, 1}
	float4 color;
};

struct vsIn {
	float4 pos : SV_POSITION;
};
struct psIn {
	float4 pos : SV_POSITION;
	uint view_id : SV_RenderTargetArrayIndex;
};

psIn vs(vsIn input, uint id : SV_InstanceID) {
	psIn output;
	output.pos     = mul(mul(input.pos, sk_inst[id].world), sk_viewproj[sk_inst[id].view_id]);
	output.view_id = sk_inst[id].view_id;
	return output;
}
float4 ps(psIn input) : SV_TARGET{
	return color;
}";

		SpatialStationaryFrameOfReference frame = null;
		SpatialSurfaceObserver            observer;
		List<SurfaceMesh>         meshes        = new List<SurfaceMesh>();
		List<SpatialSurfaceMesh>  queuedUpdates = new List<SpatialSurfaceMesh>();
		Material          surfaceMaterial;
		double            trisPerMeter;
		Vec3              center;
		float             radius = 20;

		public Material Material => surfaceMaterial;

		public OcclusionMesh(Color color, double trianglesPerMeterCubed = 2000)
		{
			// Ask or check for Spatial permissions
			SpatialSurfaceObserver.RequestAccessAsync().Completed = (i,s) => {
				if (s == AsyncStatus.Completed && i.GetResults() == SpatialPerceptionAccessStatus.Allowed) { 
					observer = new SpatialSurfaceObserver();
					observer.ObservedSurfacesChanged += OnSurfaceUpdate;
					UpdateBounds(center, radius);
				}
			};

			// Establish the coordinate frame of reference
			var locator = SpatialLocator.GetDefault();
			if (locator != null) {
				locator.LocatabilityChanged += OnLocated;
				OnLocated(locator, null);
			}

			trisPerMeter    = trianglesPerMeterCubed;
			surfaceMaterial = new Material(Shader.FromHLSL(shader));
			surfaceMaterial[MatParamName.ColorTint] = color;
		}

		private void OnLocated(SpatialLocator loc, object args)
		{
			if (frame != null || loc.Locatability != SpatialLocatability.PositionalTrackingActive) return;
			frame = loc.CreateStationaryFrameOfReferenceAtCurrentLocation();
			UpdateBounds(center, radius);
		}

		public void UpdateBounds(Vec3 center, float radius)
		{
			this.center = center;
			this.radius = radius;

			if (frame != null && observer != null) { 
				SpatialBoundingVolume bounds = SpatialBoundingVolume.FromSphere(
					frame.CoordinateSystem,
					new SpatialBoundingSphere { Center = new Vector3(center.x, center.y, center.z), Radius = radius });
				observer.SetBoundingVolume(bounds);
			}
		}

		private void OnSurfaceUpdate(SpatialSurfaceObserver observer, object args)
		{
			var surfaceDict = observer.GetObservedSurfaces();
			foreach (var surface in surfaceDict)
			{
				// Find or add a surface mesh for this surface
				SurfaceMesh mesh = meshes.Find(m=>m.id == surface.Key);
				if (mesh == null)
				{
					mesh = new SurfaceMesh {
						mesh       = new Mesh(),
						id         = surface.Key,
						lastUpdate = new DateTimeOffset() };
					meshes.Add(mesh);
				}

				// Ask for an update to the mesh data, if it's old
				if (surface.Value.UpdateTime > mesh.lastUpdate)
				{
					mesh.lastUpdate = surface.Value.UpdateTime;
					SpatialSurfaceMeshOptions opts = new SpatialSurfaceMeshOptions();
					opts.IncludeVertexNormals = false;
					opts.TriangleIndexFormat  = Windows.Graphics.DirectX.DirectXPixelFormat.R32UInt;
					opts.VertexPositionFormat = Windows.Graphics.DirectX.DirectXPixelFormat.R32G32B32A32Float;
					surface.Value.TryComputeLatestMeshAsync(trisPerMeter, opts).Completed = (info, state) => {
						// Send update to the main thread for upload to GPU
						if (state == Windows.Foundation.AsyncStatus.Completed)
							queuedUpdates.Add(info.GetResults());
					};
				}
			}
		}

		void UpdateMesh(ref Mesh mesh, SpatialSurfaceMesh src)
		{
			// Extract vertex positions from the mesh buffer
			byte[] data = new byte[ src.VertexPositions.Data.Length ];
			DataReader.FromBuffer(src.VertexPositions.Data).ReadBytes(data);
			Vertex[] verts = new Vertex[data.Length / (sizeof(float)*4)];
			int b = 0;
			for (int i = 0; i < verts.Length; i++) {
				float x = BitConverter.ToSingle(data, b); b += sizeof(float);
				float y = BitConverter.ToSingle(data, b); b += sizeof(float);
				float z = BitConverter.ToSingle(data, b); b += sizeof(float);
				b += sizeof(float);
				verts[i] = new Vertex{ pos = new Vec3(x,y,z) };
			}

			// Extract indices from the mesh buffer
			data = new byte[src.TriangleIndices.Data.Length];
			DataReader.FromBuffer(src.TriangleIndices.Data).ReadBytes(data);
			uint[] inds = new uint[data.Length/sizeof(uint)];
			for (int i = 0; i < inds.Length; i++)
			{
				inds[i] = BitConverter.ToUInt32(data, i * sizeof(uint));
			}

			// Update the StereoKit mesh
			mesh.SetVerts(verts);
			mesh.SetInds (inds);
		}

		public void Step()
		{
			// Mesh updates need to be on the main thread. There are plans to
			// make this unnecessary in the future.
			for (int i = 0; i < queuedUpdates.Count; i++)
			{
				SpatialSurfaceMesh s    = queuedUpdates[i];
				SurfaceMesh        mesh = meshes.Find(m => m.id == s.SurfaceInfo.Id);
				UpdateMesh(ref mesh.mesh, s);

				// THIS IS A HACK. It takes advantage of the fact that OpenXR
				// (local space) uses the initial head position for the 
				// origin. Our 'frame' coordinate space is created at startup,
				// so if the user remains still during initialization, then 
				// the frame coordinate space will be almost the same.
				// Any movement though, and large discrepancies can occur,
				// so this is not a long term solution.
				// A XR_MSFT_perception_anchor_interop implementation is the
				// correct way to synchronize the two spaces.
				Matrix4x4? mat = s.CoordinateSystem.TryGetTransformTo(frame.CoordinateSystem);
				if (mat.HasValue)
				{
					Matrix4x4.Decompose(mat.Value, out Vector3 sc, out Quaternion r, out Vector3 t);
					mesh.at = Matrix.TRS(new Vec3(t.X, t.Y, t.Z), new Quat(r.X, r.Y, r.Z, r.W), new Vec3(sc.X*s.VertexPositionScale.X, sc.Y*s.VertexPositionScale.Y, sc.Z*s.VertexPositionScale.Z));
				}

				// Unfortunately, this is the wrong interop API! StereoKit
				// needs to support XR_MSFT_perception_anchor_interop too.
				// Pose.FromSpatialNode uses XR_MSFT_spatial_graph_bridge
				// instead :(
				//Pose pose = Pose.FromSpatialNode(s.SurfaceInfo.Id);
				//mesh.at = pose.ToMatrix(new Vec3(s.VertexPositionScale.X, s.VertexPositionScale.Y, s.VertexPositionScale.Z));
			}
			queuedUpdates.Clear();

			// Draw all surfaces
			for (int i = 0; i < meshes.Count; i++)
				meshes[i].mesh.Draw(surfaceMaterial, meshes[i].at);
		}
	}
}

#endif