#### /Utilities/Math/AngleAxis.cs

#
C# | 319 lines | 202 code | 29 blank | 88 comment | 5 complexity | b0605d14eb3e59d2e3048d923352233b MD5 | raw file
```  1using System;
2using System.Diagnostics;
3using System.IO;
4using System.Runtime.InteropServices;
5using Delta.Utilities.Datatypes;
6using Delta.Utilities.Helpers;
7using NUnit.Framework;
8
9namespace Delta.Utilities.Math
10{
11	/// <summary>
12	/// AngleAxis helper class in case we need to manage axis calculations
13	/// (e.g. coming from a 3D package). Not used much, our code uses mostly
14	/// vectors, quaternions or just matrices.
15	/// </summary>
16	[StructLayout(LayoutKind.Explicit)]
17	[DebuggerDisplay("AngleAxis(Angle={Angle}, Axis=({Axis.X}, {Axis.Y}, " +
18	                 "{Axis.Z})")]
19	public struct AxisAngle : ISaveLoadBinary, IEquatable<AxisAngle>
20	{
21		#region Constants
22		/// <summary>
23		/// Represents the size in bytes of each AxisAngle (4 * 4 = 16 bytes).
24		/// </summary>
25		public const int DataSize = 4 * 4;
26
27		/// <summary>
28		/// Returns a AxisAngle with all values filled to zero
29		/// </summary>
30		public static readonly AxisAngle Zero =
31			new AxisAngle(0, 0, 0, 0);
32		#endregion
33
34		#region Axis (Public)
35		/// <summary>
36		/// Axis we want to rotate around, initially not set (all zero).
37		/// </summary>
38		[FieldOffset(0)]
39		public Vector Axis;
40		#endregion
41
42		#region Angle (Public)
43		/// <summary>
44		/// Rotation angle around axis
45		/// </summary>
46		[FieldOffset(12)]
47		public float Angle;
48		#endregion
49
50		#region LengthSquared (Public)
51		/// <summary>
52		/// Square Length of the axis
53		/// </summary>
54		public float LengthSquared
55		{
56			get
57			{
58				return Axis.LengthSquared;
59			}
60		}
61		#endregion
62
63		#region Length (Public)
64		/// <summary>
65		/// Length of the axis
66		/// </summary>
67		public float Length
68		{
69			get
70			{
71				return Axis.Length;
72			}
73		}
74		#endregion
75
76		#region IsNormalized (Public)
77		/// <summary>
78		/// Is the axis vector normalized?
79		/// </summary>
80		public bool IsNormalized
81		{
82			get
83			{
84				return Axis.IsNormalized;
85			}
86		}
87		#endregion
88
89		#region Constructors
90		/// <summary>
91		/// Create angle axis
92		/// </summary>
93		public AxisAngle(float setX, float setY, float setZ, float setAngle)
94		{
95			Axis = new Vector(setX, setY, setZ);
96			Angle = setAngle;
97		}
98
99		/// <summary>
100		/// Create angle axis, first 3 values are for the axis and the 4th one
101		/// is for the angle.
102		/// </summary>
103		public AxisAngle(float[] setFromArray)
104		{
105			Axis = new Vector(setFromArray[0], setFromArray[1], setFromArray[2]);
106			Angle = setFromArray[3];
107		}
108
109		/// <summary>
110		/// Create angle axis
111		/// </summary>
112		public AxisAngle(Vector setAxis, float setAngle)
113		{
114			Axis = setAxis;
115			Angle = setAngle;
116		}
117
118		/// <summary>
119		/// Create angle axis
120		/// </summary>
121		public AxisAngle(Quaternion setFromQuaternion)
122		{
123			Angle = (float)System.Math.Acos(setFromQuaternion.W);
124			float sinAngle = (float)System.Math.Sin(Angle);
125			Angle *= 2.0f;
126
127			setFromQuaternion.Normalize();
128
129			if (System.Math.Abs(sinAngle) >
130			    float.Epsilon)
131			{
132				Axis = setFromQuaternion.Vector / sinAngle;
133			}
134			else
135			{
136				Axis = Vector.Zero;
137			}
138		}
139		#endregion
140
141		#region IEquatable<AxisAngle> Members
142		/// <summary>
143		/// Equals, quickly checks if another AxisAngle has the exact same values.
144		/// </summary>
145		public bool Equals(AxisAngle other)
146		{
147			return Axis == other.Axis &&
148			       Angle == other.Angle;
149		}
150		#endregion
151
153		/// <summary>
154		/// Load axis vector and angle from a stream.
155		/// </summary>
157		{
160		}
161
162		/// <summary>
163		/// Save axis vector and angle to a stream.
164		/// </summary>
165		public void Save(BinaryWriter writer)
166		{
167			Axis.Save(writer);
168			writer.Write(Angle);
169		}
170		#endregion
171
172		#region op_Explicit (Operator)
173		/// <summary>
174		/// Op explicit
175		/// </summary>
176		public static explicit operator Quaternion(AxisAngle aa)
177		{
178			return new Quaternion(MathHelper.Sin(0.5f * aa.Angle) * aa.Axis,
179				MathHelper.Cos(0.5f * aa.Angle));
180		}
181
182		// op_Explicit()
183		/// <summary>
184		/// Op explicit
185		/// </summary>
186		public static explicit operator Matrix(AxisAngle aa)
187		{
188			return (Matrix)(Quaternion)aa;
189		}
190		#endregion
191
192		#region op_UnaryNegation (Operator)
193		/// <summary>
194		/// Arithmetic operator to negate
195		/// </summary>
196		public static AxisAngle operator -(AxisAngle aa)
197		{
198			return new AxisAngle(-aa.Axis, -aa.Angle);
199		}
200		#endregion
201
202		#region op_UnaryPlus (Operator)
203		/// <summary>
204		/// Op unary plus
205		/// </summary>
206		public static AxisAngle operator +(AxisAngle aa)
207		{
208			return new AxisAngle(aa.Axis, aa.Angle);
209		}
210		#endregion
211
213		/// <summary>
215		/// </summary>
216		public static AxisAngle operator +(AxisAngle value1, AxisAngle value2)
217		{
218			return new AxisAngle(value1.Axis + value2.Axis,
219				value1.Angle + value2.Angle);
220		}
221		#endregion
222
223		#region op_Subtraction (Operator)
224		/// <summary>
225		/// Op subtraction
226		/// </summary>
227		public static AxisAngle operator -(AxisAngle value1, AxisAngle value2)
228		{
229			return new AxisAngle(value1.Axis - value2.Axis,
230				value1.Angle - value2.Angle);
231		}
232		#endregion
233
234		#region op_Equality (Operator)
235		/// <summary>
236		/// Logic operators for equality
237		/// </summary>
238		public static bool operator ==(AxisAngle value1, AxisAngle value2)
239		{
240			return value1.Axis == value2.Axis &&
241			       System.Math.Abs(value1.Angle - value2.Angle) <= float.Epsilon;
242		}
243		#endregion
244
245		#region op_Inequality (Operator)
246		/// <summary>
247		/// Op inequality
248		/// </summary>
249		public static bool operator !=(AxisAngle value1, AxisAngle value2)
250		{
251			return value1.Axis != value2.Axis ||
252			       System.Math.Abs(value1.Angle - value2.Angle) > float.Epsilon;
253		}
254		#endregion
255
256		#region Normalize (Public)
257		/// <summary>
258		/// Normalize the axis vector
259		/// </summary>
260		public void Normalize()
261		{
262			Axis.Normalize();
263		}
264		#endregion
265
266		#region Equals (Public)
267		/// <summary>
268		/// Equals check with an object, will only return true if it is a matrix
269		/// and has the exact same values as this matrix.
270		/// </summary>
271		public override bool Equals(object obj)
272		{
273			// Note: We cannot use obj as Matrix because it is a struct, need to cast
274			return (obj is AxisAngle)
275			       	? Equals((AxisAngle)obj)
276			       	: false;
277		}
278		#endregion
279
280		#region GetHashCode (Public)
281		/// <summary>
282		/// Get hash code
283		/// </summary>
284		public override int GetHashCode()
285		{
286			return Axis.GetHashCode() ^ Angle.GetHashCode();
287		}
288		#endregion
289
290		#region CreateQuaternion (Public)
291		/// <summary>
292		/// Create from axis angle
293		/// </summary>
294		public Quaternion CreateQuaternion()
295		{
296			// Create the resulting quaternion by applying the angle
297			return new Quaternion(
298				MathHelper.Sin(Angle * 0.5f) * Axis,
299				MathHelper.Cos(Angle * 0.5f));
300		}
301		#endregion
302
303		internal class AngleAxisTests
304		{
305			#region CreateAngleAxis
306			[Test]
307			public void CreateAngleAxis()
308			{
309				AxisAngle xAxis = new AxisAngle(Vector.UnitX, 0.0f);
310				Assert.Equal(Vector.UnitX, xAxis.Axis);
311				Assert.Equal(0.0f, xAxis.Angle);
312				Assert.True(xAxis.IsNormalized);
313				Assert.Equal(new Quaternion(0, 0, 0, 1),
314					xAxis.CreateQuaternion());
315			}
316			#endregion
317		}
318	}
319}
```