context: // Quaternion.cs
  // 
  // Copyright (c) 2013 The GreenBox Development LLC, all rights reserved
  // 
  // This file is a proprietary part of GreenBox3D, disclosing the content
  // of this file without the owner consent may lead to legal actions
  using System;
  using System.Runtime.InteropServices;
  using System.ComponentModel;
  using System.Xml.Serialization;
  
  namespace GreenBox3D
  {
      /// <summary>
      ///     Represents a Quaternion.
      /// </summary>
      [Serializable]
      [StructLayout(LayoutKind.Sequential)]
      public struct Quaternion : IEquatable<Quaternion>
      {
          #region Fields
  
          private Vector3 xyz;
          private float w;
  
          #endregion
  
          #region Constructors
  
          /// <summary>
          ///     Construct a new Quaternion from vector and w components
          /// </summary>
          /// <param name="v">The vector part</param>
          /// <param name="w">The w part</param>
          public Quaternion(Vector3 v, float w)
          {
              xyz = v;
              this.w = w;
          }
  
          /// <summary>
          ///     Construct a new Quaternion
          /// </summary>
          /// <param name="x">The x component</param>
          /// <param name="y">The y component</param>
          /// <param name="z">The z component</param>
          /// <param name="w">The w component</param>
          public Quaternion(float x, float y, float z, float w)
              : this(new Vector3(x, y, z), w)
          {
          }
  
          #endregion
  
          #region Public Members
  
          #region Properties
  
          /// <summary>
          ///     Gets or sets an OpenTK.Vector3 with the X, Y and Z components of this instance.
          /// </summary>
          public Vector3 Xyz
          {
              get { return xyz; }
              set { xyz = value; }
          }
  
          /// <summary>
          ///     Gets or sets the X component of this instance.
          /// </summary>
          [XmlIgnore]
          public float X
          {
              get { return xyz.X; }
              set { xyz.X = value; }
          }
  
          /// <summary>
          ///     Gets or sets the Y component of this instance.
          /// </summary>
          [XmlIgnore]
          public float Y
          {
              get { return xyz.Y; }
              set { xyz.Y = value; }
          }
  
          /// <summary>
          ///     Gets or sets the Z component of this instance.
          /// </summary>
          [XmlIgnore]
          public float Z
          {
              get { return xyz.Z; }
              set { xyz.Z = value; }
          }
  
          /// <summary>
          ///     Gets or sets the W component of this instance.
          /// </summary>
          public float W
          {
              get { return w; }
              set { w = value; }
          }
  
          #endregion
  
          #region Instance
  
          #region ToAxisAngle
  
          /// <summary>
          ///     Convert the current quaternion to axis angle representation
          /// </summary>
          /// <param name="axis">The resultant axis</param>
          /// <param name="angle">The resultant angle</param>
          public void ToAxisAngle(out Vector3 axis, out float angle)
          {
              Vector4 result = ToAxisAngle();
              axis = result.Xyz;
              angle = result.W;
          }
  
          /// <summary>
          ///     Convert this instance to an axis-angle representation.
          /// </summary>
          /// <returns>A Vector4 that is the axis-angle representation of this quaternion.</returns>
          public Vector4 ToAxisAngle()
          {
              Quaternion q = this;
              if (Math.Abs(q.W) > 1.0f)
                  q.Normalize();
  
              Vector4 result = new Vector4();
  
              result.W = 2.0f * (float)Math.Acos(q.W); // angle
              float den = (float)Math.Sqrt(1.0 - q.W * q.W);
              if (den > 0.0001f)
              {
                  result.Xyz = q.Xyz / den;
              }
              else
              {
                  // This occurs when the angle is zero. 
                  // Not a problem: just set an arbitrary normalized axis.
                  result.Xyz = Vector3.UnitX;
              }
  
              return result;
          }
  
          #endregion
  
          #region public float Length
  
          /// <summary>
          ///     Gets the length (magnitude) of the quaternion.
          /// </summary>
          /// <seealso cref="LengthSquared" />
          public float Length
          {
              get { return (float)Math.Sqrt(W * W + Xyz.LengthSquared); }
          }
  
          #endregion
  
          #region public float LengthSquared
  
          /// <summary>
          ///     Gets the square of the quaternion length (magnitude).
          /// </summary>
          public float LengthSquared
          {
              get { return W * W + Xyz.LengthSquared; }
          }
  
          #endregion
  
          #region public void Normalize()
  
          /// <summary>
          ///     Scales the Quaternion to unit length.
          /// </summary>
          public void Normalize()
          {
              float scale = 1.0f / Length;
              Xyz *= scale;
              W *= scale;
          }
  
          #endregion
  
          #region public void Conjugate()
  
          /// <summary>
          ///     Convert this quaternion to its conjugate
          /// </summary>
          public void Conjugate()
          {
              Xyz = -Xyz;
          }
  
          #endregion
  
          #endregion
  
          #region Static
  
          #region Fields
  
          /// <summary>
          ///     Defines the identity quaternion.
          /// </summary>
          public static Quaternion Identity = new Quaternion(0, 0, 0, 1);
  
          #endregion
  
          #region Add
  
          /// <summary>
          ///     Add two quaternions
          /// </summary>
          /// <param name="left">The first operand</param>
          /// <param name="right">The second operand</param>
          /// <returns>The result of the addition</returns>
          public static Quaternion Add(Quaternion left, Quaternion right)
          {
              return new Quaternion(
                  left.Xyz + right.Xyz,
                  left.W + right.W);
          }
  
          /// <summary>
          ///     Add two quaternions
          /// </summary>
          /// <param name="left">The first operand</param>
          /// <param name="right">The second operand</param>
          /// <param name="result">The result of the addition</param>
          public static void Add(ref Quaternion left, ref Quaternion right, out Quaternion result)
          {
              result = new Quaternion(
                  left.Xyz + right.Xyz,
                  left.W + right.W);
          }
  
          #endregion
  
          #region Sub
  
          /// <summary>
          ///     Subtracts two instances.
          /// </summary>
          /// <param name="left">The left instance.</param>
          /// <param name="right">The right instance.</param>
          /// <returns>The result of the operation.</returns>
          public static Quaternion Sub(Quaternion left, Quaternion right)
          {
              return new Quaternion(
                  left.Xyz - right.Xyz,
                  left.W - right.W);
          }
  
          /// <summary>
          ///     Subtracts two instances.
          /// </summary>
          /// <param name="left">The left instance.</param>
          /// <param name="right">The right instance.</param>
          /// <param name="result">The result of the operation.</param>
          public static void Sub(ref Quaternion left, ref Quaternion right, out Quaternion result)
          {
              result = new Quaternion(
                  left.Xyz - right.Xyz,
                  left.W - right.W);
          }
  
          #endregion
  
          #region Mult
  
          /// <summary>
          ///     Multiplies two instances.
          /// </summary>
          /// <param name="left">The first instance.</param>
          /// <param name="right">The second instance.</param>
          /// <returns>A new instance containing the result of the calculation.</returns>
          [Obsolete("Use Multiply instead.")]
          public static Quaternion Mult(Quaternion left, Quaternion right)
          {
              return new Quaternion(
                  right.W * left.Xyz + left.W * right.Xyz + Vector3.Cross(left.Xyz, right.Xyz),
                  left.W * right.W - Vector3.Dot(left.Xyz, right.Xyz));
          }
  
          /// <summary>
          ///     Multiplies two instances.
          /// </summary>
          /// <param name="left">The first instance.</param>
          /// <param name="right">The second instance.</param>
          /// <param name="result">A new instance containing the result of the calculation.</param>
          [Obsolete("Use Multiply instead.")]
          public static void Mult(ref Quaternion left, ref Quaternion right, out Quaternion result)
          {
              result = new Quaternion(
                  right.W * left.Xyz + left.W * right.Xyz + Vector3.Cross(left.Xyz, right.Xyz),
                  left.W * right.W - Vector3.Dot(left.Xyz, right.Xyz));
          }
  
          /// <summary>
          ///     Multiplies two instances.
          /// </summary>
          /// <param name="left">The first instance.</param>
          /// <param name="right">The second instance.</param>
          /// <returns>A new instance containing the result of the calculation.</returns>
          public static Quaternion Multiply(Quaternion left, Quaternion right)
          {
              Quaternion result;
              Multiply(ref left, ref right, out result);
              return result;
          }
  
          /// <summary>
          ///     Multiplies two instances.
          /// </summary>
          /// <param name="left">The first instance.</param>
          /// <param name="right">The second instance.</param>
          /// <param name="result">A new instance containing the result of the calculation.</param>
          public static void Multiply(ref Quaternion left, ref Quaternion right, out Quaternion result)
          {
              result = new Quaternion(
                  right.W * left.Xyz + left.W * right.Xyz + Vector3.Cross(left.Xyz, right.Xyz),
                  left.W * right.W - Vector3.Dot(left.Xyz, right.Xyz));
          }
  
          /// <summary>
          ///     Multiplies an instance by a scalar.
          /// </summary>
          /// <param name="quaternion">The instance.</param>
          /// <param name="scale">The scalar.</param>
          /// <param name="result">A new instance containing the result of the calculation.</param>
          public static void Multiply(ref Quaternion quaternion, float scale, out Quaternion result)
          {
              result = new Quaternion(quaternion.X * scale, quaternion.Y * scale, quaternion.Z * scale,
                                      quaternion.W * scale);
          }
  
          /// <summary>
          ///     Multiplies an instance by a scalar.
          /// </summary>
          /// <param name="quaternion">The instance.</param>
          /// <param name="scale">The scalar.</param>
          /// <returns>A new instance containing the result of the calculation.</returns>
          public static Quaternion Multiply(Quaternion quaternion, float scale)
          {
              return new Quaternion(quaternion.X * scale, quaternion.Y * scale, quaternion.Z * scale, quaternion.W * scale);
          }
  
          #endregion
  
          #region Conjugate
  
          /// <summary>
          ///     Get the conjugate of the given quaternion
          /// </summary>
          /// <param name="q">The quaternion</param>
          /// <returns>The conjugate of the given quaternion</returns>
          public static Quaternion Conjugate(Quaternion q)
          {
              return new Quaternion(-q.Xyz, q.W);
          }
  
          /// <summary>
          ///     Get the conjugate of the given quaternion
          /// </summary>
          /// <param name="q">The quaternion</param>
          /// <param name="result">The conjugate of the given quaternion</param>
          public static void Conjugate(ref Quaternion q, out Quaternion result)
          {
              result = new Quaternion(-q.Xyz, q.W);
          }
  
          #endregion
  
          #region Invert
  
          /// <summary>
          ///     Get the inverse of the given quaternion
          /// </summary>
          /// <param name="q">The quaternion to invert</param>
          /// <returns>The inverse of the given quaternion</returns>
          public static Quaternion Invert(Quaternion q)
          {
              Quaternion result;
              Invert(ref q, out result);
              return result;
          }
  
          /// <summary>
          ///     Get the inverse of the given quaternion
          /// </summary>
          /// <param name="q">The quaternion to invert</param>
          /// <param name="result">The inverse of the given quaternion</param>
          public static void Invert(ref Quaternion q, out Quaternion result)
          {
              float lengthSq = q.LengthSquared;
              if (lengthSq != 0.0)
              {
                  float i = 1.0f / lengthSq;
                  result = new Quaternion(q.Xyz * -i, q.W * i);
              }
              else
              {
                  result = q;
              }
          }
  
          #endregion
  
          #region Normalize
  
          /// <summary>
          ///     Scale the given quaternion to unit length
          /// </summary>
          /// <param name="q">The quaternion to normalize</param>
          /// <returns>The normalized quaternion</returns>
          public static Quaternion Normalize(Quaternion q)
          {
              Quaternion result;
              Normalize(ref q, out result);
              return result;
          }
  
          /// <summary>
          ///     Scale the given quaternion to unit length
          /// </summary>
          /// <param name="q">The quaternion to normalize</param>
          /// <param name="result">The normalized quaternion</param>
          public static void Normalize(ref Quaternion q, out Quaternion result)
          {
              float scale = 1.0f / q.Length;
  
gt:             result = new Quaternion(q.Xyz * scale, q.W * scale);