VRMath.cs

using System;
using System.Collections.Generic;
using UnityEngine;

public static class VRMath {
	#region Vector/Quaternion Functions

	public static Quaternion FromToQuaternion(Quaternion from, Quaternion to) => to * Quaternion.Inverse(from);
	public static Vector3 EulerDelta(Quaternion from, Quaternion to) {
		var rot = to * Quaternion.Inverse(from);
		rot.ToAngleAxis(out float angle, out Vector3 axis);
		if (float.IsInfinity(axis.x))
			return Vector3.zero;
		if (angle > 180f) angle -= 360f;
		return axis * (angle * Mathf.Deg2Rad);
	}
	public static Vector3 FromToVelocity(Vector3 from, Vector3 to, float deltaTime) => (to - from) / deltaTime;
	public static Vector3 FromToAngularVelocity(Quaternion from, Quaternion to, float deltaTime) => EulerDelta(from, to) / deltaTime;

	#endregion
	#region Array Functions

	public static void ShiftArrayRight<T>(this T[] array) {
		var lastValue = array[array.Length - 1];
		for (int i = array.Length - 1; i >= 1; i--)
			array[i] = array[i - 1];
		array[0] = lastValue;
	}
	public static void ShiftArrayLeft<T>(this T[] array) {
		var firstValue = array[0];
		for (int i = 0; i < array.Length - 1; i++)
			array[i] = array[i + 1];
		array[array.Length - 1] = firstValue;
	}
	public static void SetEnabled(this MeshRenderer[] array, bool enabled) {
		for (int i = 0; i < array.Length; i++)
			if (array[i].enabled != enabled)
				array[i].enabled = enabled;
	}

	#endregion
	#region History Functions

	public static bool IsPressed (this History<bool> history) => history == true  && history.last == false;
	public static bool IsHeld    (this History<bool> history) => history == true  && history.last == true;
	public static bool IsReleased(this History<bool> history) => history == false && history.last == true;
	public static Vector3 Average(this History<Vector3> history, int frames) {
		int length = Mathf.Min(history.count, frames);
		Vector3 total = Vector3.zero;
		for (int i = 0; i < length; i++)
			total += history.values[i];
		return total / length;
	}
	public static Vector3 DistanceWeightedAverage(this History<Vector3> history, float innerRadius, float outerRadius, int frames) {
		int length = Mathf.Min(history.count, frames);
		var current = history.current;
		Vector3 total = Vector3.zero;
		float totalWeight = 0f;
		for (int i = 0; i < length; i++) {
			var value = history.values[i];
			var weight = 1f - Mathf.Clamp01((Vector3.Distance(current, value) - innerRadius) / (outerRadius - innerRadius));
			total += value * weight;
			totalWeight += weight;
		}
		return total / totalWeight;
	}

	#endregion
	#region Signed Distance Functions

	public static float SignedDistanceCube(Vector3 point, Vector3 extents) {
		Vector3 q = new Vector3(Mathf.Abs(point.x), Mathf.Abs(point.y), Mathf.Abs(point.z)) - extents;
		return new Vector3(Mathf.Max(q.x, 0f), Mathf.Max(q.y, 0f), Mathf.Max(q.z, 0f)).magnitude + Mathf.Min(Mathf.Max(q.x, Mathf.Max(q.y, q.z)), 0f);
	}
	public static float SignedDistanceCube(Vector3 point, Vector3 extents, float radius) => SignedDistanceCube(point, extents) - radius;
	public static float SignedDistanceCube(Vector3 point, Vector3 center, Vector3 extents, float radius) => SignedDistanceCube(point - center, extents, radius);
	public static float SignedDistanceCube(Vector3 point, Quaternion rotation, Vector3 extents, float radius) => SignedDistanceCube(Quaternion.Inverse(rotation) * point, extents, radius);
	public static float SignedDistanceCube(Vector3 point, Vector3 center, Quaternion rotation, Vector3 extents, float radius) => SignedDistanceCube(Quaternion.Inverse(rotation) * (point - center), extents, radius);

	#endregion
	#region Pose Functions

	public static Vector3 PosePoleArmPosition(Vector3 handPosition1, Vector3 handPosition2) => Vector3.Lerp(handPosition1, handPosition2, 0.5f);
	public static Quaternion PosePoleArmRotation(Vector3 handPosition1, Vector3 handPosition2, Vector3 upVector) {
		var offset = handPosition2 - handPosition1;
		var dir = offset.normalized;
		return Quaternion.LookRotation(dir, upVector);
	}

	#endregion
	#region Rigidbody Functions
	
	public static Rigidbody SetVelocityTarget(this Rigidbody rigidbody, Vector3 position) {
		rigidbody.velocity = FromToVelocity(rigidbody.position, position, Time.fixedDeltaTime);
		return rigidbody;
	}
	public static Rigidbody SetVelocityTarget(this Rigidbody rigidbody, Quaternion rotation) {
		rigidbody.angularVelocity = FromToAngularVelocity(rigidbody.rotation, rotation, Time.fixedDeltaTime);
		return rigidbody;
	}
	public static Rigidbody SetVelocityTarget(this Rigidbody rigidbody, Vector3 position, Quaternion rotation) {
		rigidbody.velocity = FromToVelocity(rigidbody.position, position, Time.fixedDeltaTime);
		rigidbody.angularVelocity = FromToAngularVelocity(rigidbody.rotation, rotation, Time.fixedDeltaTime);
		return rigidbody;
	}

	#endregion
}