/Utilities/Math/AngleAxis.cs

using System;

using System.Diagnostics;

using System.IO;

using System.Runtime.InteropServices;

using Delta.Utilities.Datatypes;

using Delta.Utilities.Helpers;

using NUnit.Framework;



namespace Delta.Utilities.Math

{

	/// <summary>

	/// AngleAxis helper class in case we need to manage axis calculations

	/// (e.g. coming from a 3D package). Not used much, our code uses mostly

	/// vectors, quaternions or just matrices.

	/// </summary>

	[StructLayout(LayoutKind.Explicit)]

	[DebuggerDisplay("AngleAxis(Angle={Angle}, Axis=({Axis.X}, {Axis.Y}, " +

	                 "{Axis.Z})")]

	public struct AxisAngle : ISaveLoadBinary, IEquatable<AxisAngle>

	{

		#region Constants

		/// <summary>

		/// Represents the size in bytes of each AxisAngle (4 * 4 = 16 bytes).

		/// </summary>

		public const int DataSize = 4 * 4;



		/// <summary>

		/// Returns a AxisAngle with all values filled to zero

		/// </summary>

		public static readonly AxisAngle Zero =

			new AxisAngle(0, 0, 0, 0);

		#endregion



		#region Axis (Public)

		/// <summary>

		/// Axis we want to rotate around, initially not set (all zero).

		/// </summary>

		[FieldOffset(0)]

		public Vector Axis;

		#endregion



		#region Angle (Public)

		/// <summary>

		/// Rotation angle around axis

		/// </summary>

		[FieldOffset(12)]

		public float Angle;

		#endregion



		#region LengthSquared (Public)

		/// <summary>

		/// Square Length of the axis

		/// </summary>

		public float LengthSquared

		{

			get

			{

				return Axis.LengthSquared;

			}

		}

		#endregion



		#region Length (Public)

		/// <summary>

		/// Length of the axis

		/// </summary>

		public float Length

		{

			get

			{

				return Axis.Length;

			}

		}

		#endregion



		#region IsNormalized (Public)

		/// <summary>

		/// Is the axis vector normalized?

		/// </summary>

		public bool IsNormalized

		{

			get

			{

				return Axis.IsNormalized;

			}

		}

		#endregion



		#region Constructors

		/// <summary>

		/// Create angle axis

		/// </summary>

		public AxisAngle(float setX, float setY, float setZ, float setAngle)

		{

			Axis = new Vector(setX, setY, setZ);

			Angle = setAngle;

		}



		/// <summary>

		/// Create angle axis, first 3 values are for the axis and the 4th one

		/// is for the angle.

		/// </summary>

		public AxisAngle(float[] setFromArray)

		{

			Axis = new Vector(setFromArray[0], setFromArray[1], setFromArray[2]);

			Angle = setFromArray[3];

		}



		/// <summary>

		/// Create angle axis

		/// </summary>

		public AxisAngle(Vector setAxis, float setAngle)

		{

			Axis = setAxis;

			Angle = setAngle;

		}



		/// <summary>

		/// Create angle axis

		/// </summary>

		public AxisAngle(Quaternion setFromQuaternion)

		{

			Angle = (float)System.Math.Acos(setFromQuaternion.W);

			float sinAngle = (float)System.Math.Sin(Angle);

			Angle *= 2.0f;



			setFromQuaternion.Normalize();



			if (System.Math.Abs(sinAngle) >

			    float.Epsilon)

			{

				Axis = setFromQuaternion.Vector / sinAngle;

			}

			else

			{

				Axis = Vector.Zero;

			}

		}

		#endregion



		#region IEquatable<AxisAngle> Members

		/// <summary>

		/// Equals, quickly checks if another AxisAngle has the exact same values.

		/// </summary>

		public bool Equals(AxisAngle other)

		{

			return Axis == other.Axis &&

			       Angle == other.Angle;

		}

		#endregion



		#region ISaveLoadBinary Members

		/// <summary>

		/// Load axis vector and angle from a stream.

		/// </summary>

		public void Load(BinaryReader reader)

		{

			Axis.Load(reader);

			Angle = reader.ReadSingle();

		}



		/// <summary>

		/// Save axis vector and angle to a stream.

		/// </summary>

		public void Save(BinaryWriter writer)

		{

			Axis.Save(writer);

			writer.Write(Angle);

		}

		#endregion



		#region op_Explicit (Operator)

		/// <summary>

		/// Op explicit

		/// </summary>

		public static explicit operator Quaternion(AxisAngle aa)

		{

			return new Quaternion(MathHelper.Sin(0.5f * aa.Angle) * aa.Axis,

				MathHelper.Cos(0.5f * aa.Angle));

		}



		// op_Explicit()

		/// <summary>

		/// Op explicit

		/// </summary>

		public static explicit operator Matrix(AxisAngle aa)

		{

			return (Matrix)(Quaternion)aa;

		}

		#endregion



		#region op_UnaryNegation (Operator)

		/// <summary>

		/// Arithmetic operator to negate

		/// </summary>

		public static AxisAngle operator -(AxisAngle aa)

		{

			return new AxisAngle(-aa.Axis, -aa.Angle);

		}

		#endregion



		#region op_UnaryPlus (Operator)

		/// <summary>

		/// Op unary plus

		/// </summary>

		public static AxisAngle operator +(AxisAngle aa)

		{

			return new AxisAngle(aa.Axis, aa.Angle);

		}

		#endregion



		#region op_Addition (Operator)

		/// <summary>

		/// Op addition

		/// </summary>

		public static AxisAngle operator +(AxisAngle value1, AxisAngle value2)

		{

			return new AxisAngle(value1.Axis + value2.Axis,

				value1.Angle + value2.Angle);

		}

		#endregion



		#region op_Subtraction (Operator)

		/// <summary>

		/// Op subtraction

		/// </summary>

		public static AxisAngle operator -(AxisAngle value1, AxisAngle value2)

		{

			return new AxisAngle(value1.Axis - value2.Axis,

				value1.Angle - value2.Angle);

		}

		#endregion



		#region op_Equality (Operator)

		/// <summary>

		/// Logic operators for equality

		/// </summary>

		public static bool operator ==(AxisAngle value1, AxisAngle value2)

		{

			return value1.Axis == value2.Axis &&

			       System.Math.Abs(value1.Angle - value2.Angle) <= float.Epsilon;

		}

		#endregion



		#region op_Inequality (Operator)

		/// <summary>

		/// Op inequality

		/// </summary>

		public static bool operator !=(AxisAngle value1, AxisAngle value2)

		{

			return value1.Axis != value2.Axis ||

			       System.Math.Abs(value1.Angle - value2.Angle) > float.Epsilon;

		}

		#endregion



		#region Normalize (Public)

		/// <summary>

		/// Normalize the axis vector

		/// </summary>

		public void Normalize()

		{

			Axis.Normalize();

		}

		#endregion



		#region Equals (Public)

		/// <summary>

		/// Equals check with an object, will only return true if it is a matrix

		/// and has the exact same values as this matrix.

		/// </summary>

		public override bool Equals(object obj)

		{

			// Note: We cannot use obj as Matrix because it is a struct, need to cast

			return (obj is AxisAngle)

			       	? Equals((AxisAngle)obj)

			       	: false;

		}

		#endregion



		#region GetHashCode (Public)

		/// <summary>

		/// Get hash code

		/// </summary>

		public override int GetHashCode()

		{

			return Axis.GetHashCode() ^ Angle.GetHashCode();

		}

		#endregion



		#region CreateQuaternion (Public)

		/// <summary>

		/// Create from axis angle

		/// </summary>

		public Quaternion CreateQuaternion()

		{

			// Create the resulting quaternion by applying the angle

			return new Quaternion(

				MathHelper.Sin(Angle * 0.5f) * Axis,

				MathHelper.Cos(Angle * 0.5f));

		}

		#endregion



		internal class AngleAxisTests

		{

			#region CreateAngleAxis

			[Test]

			public void CreateAngleAxis()

			{

				AxisAngle xAxis = new AxisAngle(Vector.UnitX, 0.0f);

				Assert.Equal(Vector.UnitX, xAxis.Axis);

				Assert.Equal(0.0f, xAxis.Angle);

				Assert.True(xAxis.IsNormalized);

				Assert.Equal(new Quaternion(0, 0, 0, 1),

					xAxis.CreateQuaternion());

			}

			#endregion

		}

	}

}