110 lines
3.3 KiB
C#
110 lines
3.3 KiB
C#
#region Copyright & License Information
|
|
/*
|
|
* Copyright 2007-2019 The OpenRA Developers (see AUTHORS)
|
|
* This file is part of OpenRA, which is free software. It is made
|
|
* available to you under the terms of the GNU General Public License
|
|
* as published by the Free Software Foundation, either version 3 of
|
|
* the License, or (at your option) any later version. For more
|
|
* information, see COPYING.
|
|
*/
|
|
#endregion
|
|
|
|
using System;
|
|
|
|
namespace OpenRA
|
|
{
|
|
/// <summary>
|
|
/// 3d World rotation.
|
|
/// </summary>
|
|
public struct WRot : IEquatable<WRot>
|
|
{
|
|
public readonly WAngle Roll, Pitch, Yaw;
|
|
|
|
public WRot(WAngle roll, WAngle pitch, WAngle yaw) { Roll = roll; Pitch = pitch; Yaw = yaw; }
|
|
public static readonly WRot Zero = new WRot(WAngle.Zero, WAngle.Zero, WAngle.Zero);
|
|
|
|
public static WRot FromFacing(int facing) { return new WRot(WAngle.Zero, WAngle.Zero, WAngle.FromFacing(facing)); }
|
|
public static WRot FromYaw(WAngle yaw) { return new WRot(WAngle.Zero, WAngle.Zero, yaw); }
|
|
public static WRot operator +(WRot a, WRot b) { return new WRot(a.Roll + b.Roll, a.Pitch + b.Pitch, a.Yaw + b.Yaw); }
|
|
public static WRot operator -(WRot a, WRot b) { return new WRot(a.Roll - b.Roll, a.Pitch - b.Pitch, a.Yaw - b.Yaw); }
|
|
public static WRot operator -(WRot a) { return new WRot(-a.Roll, -a.Pitch, -a.Yaw); }
|
|
|
|
public static bool operator ==(WRot me, WRot other)
|
|
{
|
|
return me.Roll == other.Roll && me.Pitch == other.Pitch && me.Yaw == other.Yaw;
|
|
}
|
|
|
|
public static bool operator !=(WRot me, WRot other) { return !(me == other); }
|
|
|
|
public WRot WithYaw(WAngle yaw)
|
|
{
|
|
return new WRot(Roll, Pitch, yaw);
|
|
}
|
|
|
|
void AsQuarternion(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);
|
|
}
|
|
|
|
public void AsMatrix(out Int32Matrix4x4 mtx)
|
|
{
|
|
int x, y, z, w;
|
|
AsQuarternion(out x, out y, out z, out w);
|
|
|
|
// Theoretically 1024 * * 2, but may differ slightly due to rounding
|
|
var lsq = x * x + y * y + z * z + w * w;
|
|
|
|
// Quaternion components use 10 bits, so there's no risk of overflow
|
|
mtx = new Int32Matrix4x4(
|
|
lsq - 2 * (y * y + z * z),
|
|
2 * (x * y + z * w),
|
|
2 * (x * z - y * w),
|
|
0,
|
|
|
|
2 * (x * y - z * w),
|
|
lsq - 2 * (x * x + z * z),
|
|
2 * (y * z + x * w),
|
|
0,
|
|
|
|
2 * (x * z + y * w),
|
|
2 * (y * z - x * w),
|
|
lsq - 2 * (x * x + y * y),
|
|
0,
|
|
|
|
0,
|
|
0,
|
|
0,
|
|
lsq);
|
|
}
|
|
|
|
public Int32Matrix4x4 AsMatrix()
|
|
{
|
|
Int32Matrix4x4 mtx;
|
|
AsMatrix(out mtx);
|
|
return mtx;
|
|
}
|
|
|
|
public override int GetHashCode() { return Roll.GetHashCode() ^ Pitch.GetHashCode() ^ Yaw.GetHashCode(); }
|
|
|
|
public bool Equals(WRot other) { return other == this; }
|
|
public override bool Equals(object obj) { return obj is WRot && Equals((WRot)obj); }
|
|
|
|
public override string ToString() { return Roll + "," + Pitch + "," + Yaw; }
|
|
}
|
|
}
|