OniSplit/Math/Quaternion.cs

using System;

namespace Oni
{
    internal struct Quaternion : IEquatable<Quaternion>
    {
        public float X;
        public float Y;
        public float Z;
        public float W;

        public Quaternion(Vector3 xyz, float w)
        {
            X = xyz.X;
            Y = xyz.Y;
            Z = xyz.Z;
            W = w;
        }

        public Quaternion(float x, float y, float z, float w)
        {
            X = x;
            Y = y;
            Z = z;
            W = w;
        }

        public Quaternion(Vector4 xyzw)
        {
            X = xyzw.X;
            Y = xyzw.Y;
            Z = xyzw.Z;
            W = xyzw.W;
        }

        private Vector3 XYZ => new Vector3(X, Y, Z);

        public static Quaternion CreateFromAxisAngle(Vector3 axis, float angle)
        {
            float halfAngle = angle * 0.5f;
            float sin = FMath.Sin(halfAngle);
            float cos = FMath.Cos(halfAngle);

            return new Quaternion(axis * sin, cos);
        }

        public void ToAxisAngle(out Vector3 axis, out float angle)
        {
            float halfAngle = FMath.Acos(W);
            float sin = FMath.Sqrt(1 - W * W);

            if (sin < 1e-5f)
            {
                axis = XYZ;
                angle = 0.0f;
            }
            else
            {
                axis = XYZ / sin;
                angle = halfAngle * 2.0f;
            }
        }

        public static Quaternion CreateFromEulerXYZ(float x, float y, float z)
        {
            x = MathHelper.ToRadians(x);
            y = MathHelper.ToRadians(y);
            z = MathHelper.ToRadians(z);

            return CreateFromAxisAngle(Vector3.UnitX, x)
                 * CreateFromAxisAngle(Vector3.UnitY, y)
                 * CreateFromAxisAngle(Vector3.UnitZ, z);
        }

        public Vector3 ToEulerXYZ()
        {
            Vector3 r;

            var p0 = -W;
            var p1 = X;
            var p2 = Y;
            var p3 = Z;
            var e = -1.0f;

            var s = 2.0f * (p0 * p2 + e * p1 * p3);

            if (s > 0.999f)
            {
                r.X = MathHelper.ToDegrees(-2.0f * (float)Math.Atan2(p1, p0));
                r.Y = -90.0f;
                r.Z = 0.0f;
            }
            else if (s < -0.999f)
            {
                r.X = MathHelper.ToDegrees(2.0f * (float)Math.Atan2(p1, p0));
                r.Y = 90.0f;
                r.Z = 0.0f;
            }
            else
            {
                r.X = -MathHelper.ToDegrees((float)Math.Atan2(2.0f * (p0 * p1 - e * p2 * p3), 1.0f - 2.0f * (p1 * p1 + p2 * p2)));
                r.Y = -MathHelper.ToDegrees((float)Math.Asin(s));
                r.Z = -MathHelper.ToDegrees((float)Math.Atan2(2.0f * (p0 * p3 - e * p1 * p2), 1.0f - 2.0f * (p2 * p2 + p3 * p3)));
            }

            return r;
        }

        public static Quaternion CreateFromYawPitchRoll(float yaw, float pitch, float roll)
        {
            float halfRoll = roll * 0.5f;
            float sinRoll = FMath.Sin(halfRoll);
            float cosRoll = FMath.Cos(halfRoll);

            float halfPitch = pitch * 0.5f;
            float sinPitch = FMath.Sin(halfPitch);
            float cosPitch = FMath.Cos(halfPitch);

            float halfYaw = yaw * 0.5f;
            float sinYaw = FMath.Sin(halfYaw);
            float cosYaw = FMath.Cos(halfYaw);

            Quaternion r;

            r.X = (cosYaw * sinPitch * cosRoll) + (sinYaw * cosPitch * sinRoll);
            r.Y = (sinYaw * cosPitch * cosRoll) - (cosYaw * sinPitch * sinRoll);
            r.Z = (cosYaw * cosPitch * sinRoll) - (sinYaw * sinPitch * cosRoll);
            r.W = (cosYaw * cosPitch * cosRoll) + (sinYaw * sinPitch * sinRoll);

            return r;
        }

        public static Quaternion CreateFromRotationMatrix(Matrix m)
        {
            Quaternion q;

            float trace = m.M11 + m.M22 + m.M33;

            if (trace > 0.0f)
            {
                float s = FMath.Sqrt(1.0f + trace);
                float inv2s = 0.5f / s;
                q.X = (m.M23 - m.M32) * inv2s;
                q.Y = (m.M31 - m.M13) * inv2s;
                q.Z = (m.M12 - m.M21) * inv2s;
                q.W = s * 0.5f;
            }
            else if (m.M11 >= m.M22 && m.M11 >= m.M33)
            {
                float s = FMath.Sqrt(1.0f + m.M11 - m.M22 - m.M33);
                float inv2s = 0.5f / s;
                q.X = s * 0.5f;
                q.Y = (m.M12 + m.M21) * inv2s;
                q.Z = (m.M13 + m.M31) * inv2s;
                q.W = (m.M23 - m.M32) * inv2s;
            }
            else if (m.M22 > m.M33)
            {
                float s = FMath.Sqrt(1.0f - m.M11 + m.M22 - m.M33);
                float inv2s = 0.5f / s;
                q.X = (m.M21 + m.M12) * inv2s;
                q.Y = s * 0.5f;
                q.Z = (m.M32 + m.M23) * inv2s;
                q.W = (m.M31 - m.M13) * inv2s;
            }
            else
            {
                float s = FMath.Sqrt(1.0f - m.M11 - m.M22 + m.M33);
                float inv2s = 0.5f / s;
                q.X = (m.M31 + m.M13) * inv2s;
                q.Y = (m.M32 + m.M23) * inv2s;
                q.Z = s * 0.5f;
                q.W = (m.M12 - m.M21) * inv2s;
            }

            return q;
        }

        public static Quaternion Lerp(Quaternion q1, Quaternion q2, float amount)
        {
            float invAmount = 1.0f - amount;

            if (Dot(q1, q2) < 0.0f)
                amount = -amount;

            q1.X = invAmount * q1.X + amount * q2.X;
            q1.Y = invAmount * q1.Y + amount * q2.Y;
            q1.Z = invAmount * q1.Z + amount * q2.Z;
            q1.W = invAmount * q1.W + amount * q2.W;

            q1.Normalize();

            return q1;
        }

        public static float Dot(Quaternion q1, Quaternion q2)
            => q1.X * q2.X + q1.Y * q2.Y + q1.Z * q2.Z + q1.W * q2.W;

        public static Quaternion operator +(Quaternion q1, Quaternion q2)
        {
            q1.X += q2.X;
            q1.Y += q2.Y;
            q1.Z += q2.Z;
            q1.W += q2.W;

            return q1;
        }

        public static Quaternion operator -(Quaternion q1, Quaternion q2)
        {
            q1.X -= q2.X;
            q1.Y -= q2.Y;
            q1.Z -= q2.Z;
            q1.W -= q2.W;

            return q1;
        }

        public static Quaternion operator *(Quaternion q1, Quaternion q2) => new Quaternion
        {
            X = q1.X * q2.W + q1.Y * q2.Z - q1.Z * q2.Y + q1.W * q2.X,
            Y = -q1.X * q2.Z + q1.Y * q2.W + q1.Z * q2.X + q1.W * q2.Y,
            Z = q1.X * q2.Y - q1.Y * q2.X + q1.Z * q2.W + q1.W * q2.Z,
            W = -q1.X * q2.X - q1.Y * q2.Y - q1.Z * q2.Z + q1.W * q2.W,
        };

        public static Quaternion operator *(Quaternion q, float s)
        {
            q.X *= s;
            q.Y *= s;
            q.Z *= s;
            q.W *= s;

            return q;
        }

        public static bool operator ==(Quaternion q1, Quaternion q2) => q1.Equals(q2);
        public static bool operator !=(Quaternion q1, Quaternion q2) => !q1.Equals(q2);

        public static Quaternion Conjugate(Quaternion q)
        {
            q.X = -q.X;
            q.Y = -q.Y;
            q.Z = -q.Z;

            return q;
        }

        public Quaternion Inverse()
        {
            float inv = 1.0f / SquaredLength();

            Quaternion r;
            r.X = -X * inv;
            r.Y = -Y * inv;
            r.Z = -Z * inv;
            r.W = W * inv;
            return r;
        }

        public void Normalize()
        {
            float f = 1.0f / Length();

            X *= f;
            Y *= f;
            Z *= f;
            W *= f;
        }

        public float Length() => FMath.Sqrt(SquaredLength());

        public float SquaredLength() => X * X + Y * Y + Z * Z + W * W;

        public bool Equals(Quaternion other) => X == other.X && Y == other.Y && Z == other.Z && W == other.W;

        public override bool Equals(object obj) => obj is Quaternion && Equals((Quaternion)obj);

        public override int GetHashCode() => X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode() ^ W.GetHashCode();

        public override string ToString() => $"{{{X} {Y} {Z} {W}}}";

        public Matrix ToMatrix()
        {
            float xx = X * X;
            float yy = Y * Y;
            float zz = Z * Z;
            float xy = X * Y;
            float zw = Z * W;
            float zx = Z * X;
            float yw = Y * W;
            float yz = Y * Z;
            float xw = X * W;

            Matrix m;

            m.M11 = 1.0f - 2.0f * (yy + zz);
            m.M12 = 2.0f * (xy + zw);
            m.M13 = 2.0f * (zx - yw);
            m.M14 = 0.0f;

            m.M21 = 2.0f * (xy - zw);
            m.M22 = 1.0f - 2.0f * (zz + xx);
            m.M23 = 2.0f * (yz + xw);
            m.M24 = 0.0f;

            m.M31 = 2.0f * (zx + yw);
            m.M32 = 2.0f * (yz - xw);
            m.M33 = 1.0f - 2.0f * (yy + xx);
            m.M34 = 0.0f;

            m.M41 = 0.0f;
            m.M42 = 0.0f;
            m.M43 = 0.0f;
            m.M44 = 1.0f;

            return m;
        }

        public Vector4 ToVector4() => new Vector4(X, Y, Z, W);

        private static readonly Quaternion identity = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);

        public static Quaternion Identity => identity;
    }
}
Revision:	1114
Committed:	Wed Jan 22 14:08:57 2020 UTC (5 years, 9 months ago) by iritscen
File size:	9773 byte(s)
Log Message:	Adding OniSplit source code (v0.9.99.0). Many thanks to Neo for all his work over the years.
#	Content
1	using System;
2
3	namespace Oni
4	{
5	internal struct Quaternion : IEquatable<Quaternion>
6	{
7	public float X;
8	public float Y;
9	public float Z;
10	public float W;
11
12	public Quaternion(Vector3 xyz, float w)
13	{
14	X = xyz.X;
15	Y = xyz.Y;
16	Z = xyz.Z;
17	W = w;
18	}
19
20	public Quaternion(float x, float y, float z, float w)
21	{
22	X = x;
23	Y = y;
24	Z = z;
25	W = w;
26	}
27
28	public Quaternion(Vector4 xyzw)
29	{
30	X = xyzw.X;
31	Y = xyzw.Y;
32	Z = xyzw.Z;
33	W = xyzw.W;
34	}
35
36	private Vector3 XYZ => new Vector3(X, Y, Z);
37
38	public static Quaternion CreateFromAxisAngle(Vector3 axis, float angle)
39	{
40	float halfAngle = angle * 0.5f;
41	float sin = FMath.Sin(halfAngle);
42	float cos = FMath.Cos(halfAngle);
43
44	return new Quaternion(axis * sin, cos);
45	}
46
47	public void ToAxisAngle(out Vector3 axis, out float angle)
48	{
49	float halfAngle = FMath.Acos(W);
50	float sin = FMath.Sqrt(1 - W * W);
51
52	if (sin < 1e-5f)
53	{
54	axis = XYZ;
55	angle = 0.0f;
56	}
57	else
58	{
59	axis = XYZ / sin;
60	angle = halfAngle * 2.0f;
61	}
62	}
63
64	public static Quaternion CreateFromEulerXYZ(float x, float y, float z)
65	{
66	x = MathHelper.ToRadians(x);
67	y = MathHelper.ToRadians(y);
68	z = MathHelper.ToRadians(z);
69
70	return CreateFromAxisAngle(Vector3.UnitX, x)
71	* CreateFromAxisAngle(Vector3.UnitY, y)
72	* CreateFromAxisAngle(Vector3.UnitZ, z);
73	}
74
75	public Vector3 ToEulerXYZ()
76	{
77	Vector3 r;
78
79	var p0 = -W;
80	var p1 = X;
81	var p2 = Y;
82	var p3 = Z;
83	var e = -1.0f;
84
85	var s = 2.0f * (p0 * p2 + e * p1 * p3);
86
87	if (s > 0.999f)
88	{
89	r.X = MathHelper.ToDegrees(-2.0f * (float)Math.Atan2(p1, p0));
90	r.Y = -90.0f;
91	r.Z = 0.0f;
92	}
93	else if (s < -0.999f)
94	{
95	r.X = MathHelper.ToDegrees(2.0f * (float)Math.Atan2(p1, p0));
96	r.Y = 90.0f;
97	r.Z = 0.0f;
98	}
99	else
100	{
101	r.X = -MathHelper.ToDegrees((float)Math.Atan2(2.0f * (p0 * p1 - e * p2 * p3), 1.0f - 2.0f * (p1 * p1 + p2 * p2)));
102	r.Y = -MathHelper.ToDegrees((float)Math.Asin(s));
103	r.Z = -MathHelper.ToDegrees((float)Math.Atan2(2.0f * (p0 * p3 - e * p1 * p2), 1.0f - 2.0f * (p2 * p2 + p3 * p3)));
104	}
105
106	return r;
107	}
108
109	public static Quaternion CreateFromYawPitchRoll(float yaw, float pitch, float roll)
110	{
111	float halfRoll = roll * 0.5f;
112	float sinRoll = FMath.Sin(halfRoll);
113	float cosRoll = FMath.Cos(halfRoll);
114
115	float halfPitch = pitch * 0.5f;
116	float sinPitch = FMath.Sin(halfPitch);
117	float cosPitch = FMath.Cos(halfPitch);
118
119	float halfYaw = yaw * 0.5f;
120	float sinYaw = FMath.Sin(halfYaw);
121	float cosYaw = FMath.Cos(halfYaw);
122
123	Quaternion r;
124
125	r.X = (cosYaw * sinPitch * cosRoll) + (sinYaw * cosPitch * sinRoll);
126	r.Y = (sinYaw * cosPitch * cosRoll) - (cosYaw * sinPitch * sinRoll);
127	r.Z = (cosYaw * cosPitch * sinRoll) - (sinYaw * sinPitch * cosRoll);
128	r.W = (cosYaw * cosPitch * cosRoll) + (sinYaw * sinPitch * sinRoll);
129
130	return r;
131	}
132
133	public static Quaternion CreateFromRotationMatrix(Matrix m)
134	{
135	Quaternion q;
136
137	float trace = m.M11 + m.M22 + m.M33;
138
139	if (trace > 0.0f)
140	{
141	float s = FMath.Sqrt(1.0f + trace);
142	float inv2s = 0.5f / s;
143	q.X = (m.M23 - m.M32) * inv2s;
144	q.Y = (m.M31 - m.M13) * inv2s;
145	q.Z = (m.M12 - m.M21) * inv2s;
146	q.W = s * 0.5f;
147	}
148	else if (m.M11 >= m.M22 && m.M11 >= m.M33)
149	{
150	float s = FMath.Sqrt(1.0f + m.M11 - m.M22 - m.M33);
151	float inv2s = 0.5f / s;
152	q.X = s * 0.5f;
153	q.Y = (m.M12 + m.M21) * inv2s;
154	q.Z = (m.M13 + m.M31) * inv2s;
155	q.W = (m.M23 - m.M32) * inv2s;
156	}
157	else if (m.M22 > m.M33)
158	{
159	float s = FMath.Sqrt(1.0f - m.M11 + m.M22 - m.M33);
160	float inv2s = 0.5f / s;
161	q.X = (m.M21 + m.M12) * inv2s;
162	q.Y = s * 0.5f;
163	q.Z = (m.M32 + m.M23) * inv2s;
164	q.W = (m.M31 - m.M13) * inv2s;
165	}
166	else
167	{
168	float s = FMath.Sqrt(1.0f - m.M11 - m.M22 + m.M33);
169	float inv2s = 0.5f / s;
170	q.X = (m.M31 + m.M13) * inv2s;
171	q.Y = (m.M32 + m.M23) * inv2s;
172	q.Z = s * 0.5f;
173	q.W = (m.M12 - m.M21) * inv2s;
174	}
175
176	return q;
177	}
178
179	public static Quaternion Lerp(Quaternion q1, Quaternion q2, float amount)
180	{
181	float invAmount = 1.0f - amount;
182
183	if (Dot(q1, q2) < 0.0f)
184	amount = -amount;
185
186	q1.X = invAmount * q1.X + amount * q2.X;
187	q1.Y = invAmount * q1.Y + amount * q2.Y;
188	q1.Z = invAmount * q1.Z + amount * q2.Z;
189	q1.W = invAmount * q1.W + amount * q2.W;
190
191	q1.Normalize();
192
193	return q1;
194	}
195
196	public static float Dot(Quaternion q1, Quaternion q2)
197	=> q1.X * q2.X + q1.Y * q2.Y + q1.Z * q2.Z + q1.W * q2.W;
198
199	public static Quaternion operator +(Quaternion q1, Quaternion q2)
200	{
201	q1.X += q2.X;
202	q1.Y += q2.Y;
203	q1.Z += q2.Z;
204	q1.W += q2.W;
205
206	return q1;
207	}
208
209	public static Quaternion operator -(Quaternion q1, Quaternion q2)
210	{
211	q1.X -= q2.X;
212	q1.Y -= q2.Y;
213	q1.Z -= q2.Z;
214	q1.W -= q2.W;
215
216	return q1;
217	}
218
219	public static Quaternion operator *(Quaternion q1, Quaternion q2) => new Quaternion
220	{
221	X = q1.X * q2.W + q1.Y * q2.Z - q1.Z * q2.Y + q1.W * q2.X,
222	Y = -q1.X * q2.Z + q1.Y * q2.W + q1.Z * q2.X + q1.W * q2.Y,
223	Z = q1.X * q2.Y - q1.Y * q2.X + q1.Z * q2.W + q1.W * q2.Z,
224	W = -q1.X * q2.X - q1.Y * q2.Y - q1.Z * q2.Z + q1.W * q2.W,
225	};
226
227	public static Quaternion operator *(Quaternion q, float s)
228	{
229	q.X *= s;
230	q.Y *= s;
231	q.Z *= s;
232	q.W *= s;
233
234	return q;
235	}
236
237	public static bool operator ==(Quaternion q1, Quaternion q2) => q1.Equals(q2);
238	public static bool operator !=(Quaternion q1, Quaternion q2) => !q1.Equals(q2);
239
240	public static Quaternion Conjugate(Quaternion q)
241	{
242	q.X = -q.X;
243	q.Y = -q.Y;
244	q.Z = -q.Z;
245
246	return q;
247	}
248
249	public Quaternion Inverse()
250	{
251	float inv = 1.0f / SquaredLength();
252
253	Quaternion r;
254	r.X = -X * inv;
255	r.Y = -Y * inv;
256	r.Z = -Z * inv;
257	r.W = W * inv;
258	return r;
259	}
260
261	public void Normalize()
262	{
263	float f = 1.0f / Length();
264
265	X *= f;
266	Y *= f;
267	Z *= f;
268	W *= f;
269	}
270
271	public float Length() => FMath.Sqrt(SquaredLength());
272
273	public float SquaredLength() => X * X + Y * Y + Z * Z + W * W;
274
275	public bool Equals(Quaternion other) => X == other.X && Y == other.Y && Z == other.Z && W == other.W;
276
277	public override bool Equals(object obj) => obj is Quaternion && Equals((Quaternion)obj);
278
279	public override int GetHashCode() => X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode() ^ W.GetHashCode();
280
281	public override string ToString() => $"{{{X} {Y} {Z} {W}}}";
282
283	public Matrix ToMatrix()
284	{
285	float xx = X * X;
286	float yy = Y * Y;
287	float zz = Z * Z;
288	float xy = X * Y;
289	float zw = Z * W;
290	float zx = Z * X;
291	float yw = Y * W;
292	float yz = Y * Z;
293	float xw = X * W;
294
295	Matrix m;
296
297	m.M11 = 1.0f - 2.0f * (yy + zz);
298	m.M12 = 2.0f * (xy + zw);
299	m.M13 = 2.0f * (zx - yw);
300	m.M14 = 0.0f;
301
302	m.M21 = 2.0f * (xy - zw);
303	m.M22 = 1.0f - 2.0f * (zz + xx);
304	m.M23 = 2.0f * (yz + xw);
305	m.M24 = 0.0f;
306
307	m.M31 = 2.0f * (zx + yw);
308	m.M32 = 2.0f * (yz - xw);
309	m.M33 = 1.0f - 2.0f * (yy + xx);
310	m.M34 = 0.0f;
311
312	m.M41 = 0.0f;
313	m.M42 = 0.0f;
314	m.M43 = 0.0f;
315	m.M44 = 1.0f;
316
317	return m;
318	}
319
320	public Vector4 ToVector4() => new Vector4(X, Y, Z, W);
321
322	private static readonly Quaternion identity = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
323
324	public static Quaternion Identity => identity;
325	}
326	}