source: OniSplit/Math/Quaternion.cs

Last change on this file was 1114, checked in by iritscen, 5 years ago

Adding OniSplit source code (v0.9.99.0). Many thanks to Neo for all his work over the years.
File size: 9.5 KB
Line 
1using System;
2
3namespace 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}
Note: See TracBrowser for help on using the repository browser.