/Utilities/Math/AngleAxis.cs
C# | 319 lines | 202 code | 29 blank | 88 comment | 5 complexity | b0605d14eb3e59d2e3048d923352233b MD5 | raw file
Possible License(s): Apache-2.0
- 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
- }
- }
- }