diff --git a/OpenRA.Game/WRot.cs b/OpenRA.Game/WRot.cs index b469c36a69..0611b0ea18 100644 --- a/OpenRA.Game/WRot.cs +++ b/OpenRA.Game/WRot.cs @@ -18,9 +18,57 @@ namespace OpenRA /// public struct WRot : IEquatable { + // The Euler angle representation is a lot more intuitive for public use public readonly WAngle Roll, Pitch, Yaw; - public WRot(WAngle roll, WAngle pitch, WAngle yaw) { Roll = roll; Pitch = pitch; Yaw = yaw; } + // Internal calculations use the quaternion form + readonly int x, y, z, w; + + public WRot(WAngle roll, WAngle pitch, WAngle yaw) + { + Roll = roll; + Pitch = pitch; + Yaw = yaw; + + // Angles increase clockwise + var qr = new WAngle(-Roll.Angle / 2); + var qp = new WAngle(-Pitch.Angle / 2); + var qy = new WAngle(-Yaw.Angle / 2); + var cr = (long)qr.Cos(); + var sr = (long)qr.Sin(); + var cp = (long)qp.Cos(); + var sp = (long)qp.Sin(); + var cy = (long)qy.Cos(); + var sy = (long)qy.Sin(); + + // Normalized to 1024 == 1.0 + x = (int)((sr * cp * cy - cr * sp * sy) / 1048576); + y = (int)((cr * sp * cy + sr * cp * sy) / 1048576); + z = (int)((cr * cp * sy - sr * sp * cy) / 1048576); + w = (int)((cr * cp * cy + sr * sp * sy) / 1048576); + } + + WRot(int x, int y, int z, int w) + { + this.x = x; + this.y = y; + this.z = z; + this.w = w; + + // Theoretically 1024 squared, but may differ slightly due to rounding + var lsq = x * x + y * y + z * z + w * w; + + var srcp = 2 * (w * x + y * z); + var crcp = lsq - 2 * (x * x + y * y); + var sp = (w * y - z * x) / 512; + 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); + } + public static readonly WRot None = new WRot(WAngle.Zero, WAngle.Zero, WAngle.Zero); public static WRot FromFacing(int facing) { return new WRot(WAngle.Zero, WAngle.Zero, WAngle.FromFacing(facing)); } @@ -37,17 +85,12 @@ namespace OpenRA if (rot == None) return this; - int x1, y1, z1, w1; - int x2, y2, z2, w2; - rot.AsQuaternion(out x1, out y1, out z1, out w1); - AsQuaternion(out x2, out y2, out z2, out w2); + var rx = ((long)rot.w * x + (long)rot.x * w + (long)rot.y * z - (long)rot.z * y) / 1024; + var ry = ((long)rot.w * y - (long)rot.x * z + (long)rot.y * w + (long)rot.z * x) / 1024; + var rz = ((long)rot.w * z + (long)rot.x * y - (long)rot.y * x + (long)rot.z * w) / 1024; + var rw = ((long)rot.w * w - (long)rot.x * x - (long)rot.y * y - (long)rot.z * z) / 1024; - var x3 = ((long)w1 * x2 + (long)x1 * w2 + (long)y1 * z2 - (long)z1 * y2) / 1024; - var y3 = ((long)w1 * y2 - (long)x1 * z2 + (long)y1 * w2 + (long)z1 * x2) / 1024; - var z3 = ((long)w1 * z2 + (long)x1 * y2 - (long)y1 * x2 + (long)z1 * w2) / 1024; - var w3 = ((long)w1 * w2 - (long)x1 * x2 - (long)y1 * y2 - (long)z1 * z2) / 1024; - - return FromQuaternion((int)x3, (int)y3, (int)z3, (int)w3); + return new WRot((int)rx, (int)ry, (int)rz, (int)rw); } public static bool operator ==(WRot me, WRot other) @@ -72,48 +115,8 @@ namespace OpenRA return new WRot(Roll, Pitch, yaw); } - void AsQuaternion(out int x, out int y, out int z, out int w) - { - // Angles increase clockwise - var roll = new WAngle(-Roll.Angle / 2); - var pitch = new WAngle(-Pitch.Angle / 2); - var yaw = new WAngle(-Yaw.Angle / 2); - var cr = (long)roll.Cos(); - var sr = (long)roll.Sin(); - var cp = (long)pitch.Cos(); - var sp = (long)pitch.Sin(); - var cy = (long)yaw.Cos(); - var sy = (long)yaw.Sin(); - - // Normalized to 1024 == 1.0 - x = (int)((sr * cp * cy - cr * sp * sy) / 1048576); - y = (int)((cr * sp * cy + sr * cp * sy) / 1048576); - z = (int)((cr * cp * sy - sr * sp * cy) / 1048576); - w = (int)((cr * cp * cy + sr * sp * sy) / 1048576); - } - - static WRot FromQuaternion(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; - - var srcp = 2 * (w * x + y * z); - var crcp = lsq - 2 * (x * x + y * y); - var sp = (w * y - z * x) / 512; - var sycp = 2 * (w * z + x * y); - var cycp = lsq - 2 * (y * y + z * z); - - 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 new WRot(-roll, -pitch, -yaw); - } - public void AsMatrix(out Int32Matrix4x4 mtx) { - int x, y, z, w; - AsQuaternion(out x, out y, out z, out w); - // Theoretically 1024 squared, but may differ slightly due to rounding var lsq = x * x + y * y + z * z + w * w;