From c88d1bbefa684d1a239d7480b59c2c787684b0a7 Mon Sep 17 00:00:00 2001
From: Paul Chote <pchote@users.noreply.github.com>
Date: Sat, 21 Aug 2021 10:17:11 +0100
Subject: [PATCH] Add WRot SLerp and axis-angle ctor.

These allow more complex calculations to be made
using rotations.
---
 OpenRA.Game/WRot.cs | 58 ++++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 55 insertions(+), 3 deletions(-)

diff --git a/OpenRA.Game/WRot.cs b/OpenRA.Game/WRot.cs
index e35f62b276..a6b2ed2e2d 100644
--- a/OpenRA.Game/WRot.cs
+++ b/OpenRA.Game/WRot.cs
@@ -24,6 +24,9 @@ namespace OpenRA
 		// Internal calculations use the quaternion form
 		readonly int x, y, z, w;
 
+		/// <summary>
+		/// Construct a rotation from euler angles.
+		/// </summary>
 		public WRot(WAngle roll, WAngle pitch, WAngle yaw)
 		{
 			Roll = roll;
@@ -48,6 +51,21 @@ namespace OpenRA
 			w = (int)((cr * cp * cy + sr * sp * sy) / 1048576);
 		}
 
+		/// <summary>
+		/// Construct a rotation from an axis and angle.
+		/// The axis is expected to be normalized to length 1024
+		/// </summary>
+		public WRot(WVec axis, WAngle angle)
+		{
+			// Angles increase clockwise
+			x = axis.X * new WAngle(-angle.Angle / 2).Sin() / 1024;
+			y = axis.Y * new WAngle(-angle.Angle / 2).Sin() / 1024;
+			z = axis.Z * new WAngle(-angle.Angle / 2).Sin() / 1024;
+			w = new WAngle(-angle.Angle / 2).Cos();
+
+			(Roll, Pitch, Yaw) = QuaternionToEuler(x, y, z, w);
+		}
+
 		WRot(int x, int y, int z, int w)
 		{
 			this.x = x;
@@ -55,6 +73,11 @@ namespace OpenRA
 			this.z = z;
 			this.w = w;
 
+			(Roll, Pitch, Yaw) = QuaternionToEuler(x, y, z, w);
+		}
+
+		static (WAngle, WAngle, WAngle) QuaternionToEuler(int x, int y, int z, int w)
+		{
 			// Theoretically 1024 squared, but may differ slightly due to rounding
 			var lsq = x * x + y * y + z * z + w * w;
 
@@ -64,9 +87,11 @@ namespace OpenRA
 			var sycp = 2 * (w * z + x * y);
 			var cycp = lsq - 2 * (y * y + z * z);
 
-			Roll = -WAngle.ArcTan(srcp, crcp);
-			Pitch = -(Math.Abs(sp) >= 1024 ? new WAngle(Math.Sign(sp) * 256) : WAngle.ArcSin(sp));
-			Yaw = -WAngle.ArcTan(sycp, cycp);
+			var roll = -WAngle.ArcTan(srcp, crcp);
+			var pitch = -(Math.Abs(sp) >= 1024 ? new WAngle(Math.Sign(sp) * 256) : WAngle.ArcSin(sp));
+			var yaw = -WAngle.ArcTan(sycp, cycp);
+
+			return (roll, pitch, yaw);
 		}
 
 		WRot(int x, int y, int z, int w, WAngle roll, WAngle pitch, WAngle yaw)
@@ -168,5 +193,32 @@ namespace OpenRA
 		public override bool Equals(object obj) { return obj is WRot && Equals((WRot)obj); }
 
 		public override string ToString() { return Roll + "," + Pitch + "," + Yaw; }
+
+		public static WRot SLerp(in WRot a, in WRot b, int mul, int div)
+		{
+			// This implements the standard spherical linear interpolation
+			// between two quaternions, accounting for OpenRA's integer math
+			// conventions and WRot always using (nearly) normalized quaternions
+			var dot = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
+			var flip = dot >= 0 ? 1 : -1;
+
+			// a and b describe the same rotation
+			if (flip * dot >= 1024 * 1024)
+				return a;
+
+			var theta = WAngle.ArcCos(dot / 1024);
+			var s1 = new WAngle((div - mul) * theta.Angle / div).Sin();
+			var s2 = new WAngle(mul * theta.Angle / div).Sin();
+			var s3 = theta.Sin();
+
+			var x = ((long)a.x * s1 + flip * b.x * s2) / s3;
+			var y = ((long)a.y * s1 + flip * b.y * s2) / s3;
+			var z = ((long)a.z * s1 + flip * b.z * s2) / s3;
+			var w = ((long)a.w * s1 + flip * b.w * s2) / s3;
+
+			// Normalize to 1024 == 1.0
+			var l = Exts.ISqrt(x * x + y * y + z * z + w * w);
+			return new WRot((int)(1024 * x / l), (int)(1024 * y / l), (int)(1024 * z / l), (int)(1024 * w / l));
+		}
 	}
 }