epic/OpenRA
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Store quaternion components directly.

Browse Source 
This avoids precision loss when combining rotations.
The equivalent Euler angles are calculated for external use
but the quaternion components are preferred for any further
internal calculations.
This commit is contained in:
Paul Chote
2020-07-12 14:32:06 +01:00
committed by tovl
parent aae497eff1
commit b417b267dd
1 changed files with 54 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

105
OpenRA.Game/WRot.cs
View File

@@ -18,9 +18,57 @@ namespace OpenRA
/// </summary>
public struct WRot : IEquatable<WRot>
{
// 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;