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;
+ ///
+ /// Construct a rotation from euler angles.
+ ///
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);
}
+ ///
+ /// Construct a rotation from an axis and angle.
+ /// The axis is expected to be normalized to length 1024
+ ///
+ 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));
+ }
}
}