A partial StereoKit camera controller

Camera.cs

using System;

namespace StereoKit.Framework
{
	class Camera : IStepper
	{
		public delegate bool IntersectionDelegate(Ray worldRay, out Ray worldIntersection);

		float  _headHeight = 1.5f;
		float  _floor      = 0;
		Vec2   _rotation   = new Vec2(0,0);
		float  _prevStick  = 0;

		IntersectionDelegate IntersectWorld;

		public Vec3  Position         { get; set; }
		public float Speed            { get; set; } = 2;
		public float RunSpeed         { get; set; } = 4;
		public float MouseSensitivity { get; set; } = .25f;
		public float SnapIncrement    { get; set; } = 30;
		public bool  Grounded         { get; set; } = true;
		public bool  Enabled => true;

		public Camera(IntersectionDelegate intersectWorldDelegate)
		{
			IntersectWorld = intersectWorldDelegate;
		}

		public bool Initialize()
		{
			if (World.HasBounds)
			{
				_floor      = World.BoundsPose.position.y;
				_headHeight = Input.Head.position.y - _floor;
			}
			else
			{
				_headHeight = 1.5f;
				_floor      = Input.Head.position.y - _headHeight;
			}
			return true;
		}

		public void Step()
		{
			// Raycast teleportation
			Pointer p         = Input.Pointer(0, InputSource.HandRight);
			Vec3    fingertip = Input.Hand(Handed.Right)[FingerId.Index, JointId.Tip].position;
			p.ray           = new Ray(fingertip, fingertip - Input.Head.position);
			p.state         = Input.Hand(Handed.Left).grip;
			p.ray.direction = p.ray.direction.Normalized;
			if ((p.state & (BtnState.Active | BtnState.JustInactive)) > 0)
			{
				Lines.Add(p.ray.position, p.ray.position + p.ray.direction * 2, Color32.White, new Color32(255, 255, 255, 0), 0.02f);
				if (IntersectWorld(p.ray, out Ray at))
				{
					Lines.Add(at.position, at.position + Vec3.Up, Color32.White, new Color32(255, 255, 255, 0), 0.2f);
					if (p.state.IsJustInactive())
					{
						Position = at.position + V.XYZ(0, _headHeight, 0);
					}
				}
			}

			// Keyboard/controller stick movement
			Vec2 move = Vec2.Zero;

			// If the MR sim is enabled, it has some of its own keyboard/mouse
			// movement code.
			if (SK.Settings.disableFlatscreenMRSim)
			{
				if (Input.Key(Key.W).IsActive()) move.y += 1;
				if (Input.Key(Key.S).IsActive()) move.y -= 1;
				if (Input.Key(Key.D).IsActive()) move.x += 1;
				if (Input.Key(Key.A).IsActive()) move.x -= 1;
				if (Input.Key(Key.MouseRight).IsActive())
				{
					_rotation.y -= Input.Mouse.posChange.x * MouseSensitivity;
					_rotation.x -= Input.Mouse.posChange.y * MouseSensitivity * 0.7f;
				}
			}

			// Controller movement input
			Controller leftController = Input.Controller(Handed.Left);
			if (leftController.stick.MagnitudeSq > 0.01f)
			{
				move   = leftController.stick;
				move.x = -move.x;
			}
			float rightStick = Input.Controller(Handed.Right).stick.x;
			if (Math.Abs(rightStick) >= 0.75f && _prevStick < 0.75f)
			{
				// Rotation here should happen around the Head pose, rather
				// than around the center position
				_rotation.y += Math.Sign(rightStick) * SnapIncrement;
			}
			_prevStick = Math.Abs(rightStick);

			// Apply movement data to the position
			if (move.LengthSq > 1)
				move.Normalize();

			bool  running = Input.Key(Key.Shift).IsActive() || leftController.IsStickClicked;
			float speed   = (running ? RunSpeed : Speed) * Time.Elapsedf;

			Quat orientation = Input.Head.orientation;
			Position += orientation.Rotate(Vec3.Forward) * move.y * speed;
			Position += orientation.Rotate(Vec3.Right  ) * move.x * speed;

			// Update the floor value
			if (Grounded == true)
			{
				if (move.LengthSq > 0.0001f && IntersectWorld(new Ray(Position, -Vec3.UnitY), out Ray floorAt))
				{
					_floor = floorAt.position.y;
				}
				Position = new Vec3(Position.x, _floor + _headHeight, Position.z);
			}
			Renderer.CameraRoot = Matrix.TR(Position, Quat.FromAngles(_rotation.x, _rotation.y, 0));

			Hierarchy.Push(Renderer.CameraRoot);
			Lines.Add(V.XYZ(0, _floor, 0), V.XYZ(0, _floor+0.5f, 0), Color.White, 0.01f);;
			Hierarchy.Pop();
		}

		public void Shutdown() { }
	}
}