PageRenderTime 165ms CodeModel.GetById 82ms app.highlight 49ms RepoModel.GetById 31ms app.codeStats 0ms

/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
152		#region ISaveLoadBinary Members
153		/// <summary>
154		/// Load axis vector and angle from a stream.
155		/// </summary>
156		public void Load(BinaryReader reader)
157		{
158			Axis.Load(reader);
159			Angle = reader.ReadSingle();
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
212		#region op_Addition (Operator)
213		/// <summary>
214		/// Op addition
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}