Quaternion.cs

using System;

namespace MathNet.Numerics
{
	public struct Quaternion64 {
		public double w, x, y, z;

		public double NormSquared {
			get {
				return w * w + x * x + y * y + z * z;
			}
		}

		public double Norm {
			get {
				return Math.Sqrt (this.NormSquared);
			}
		}

		/// <summary>
		/// Normalize the quaternion in-place so it has a norm of 1.0.
		/// </summary>
		public void Normalize() {
			var invNorm = 1.0 / this.Norm;
			this.w *= invNorm;
			this.x *= invNorm;
			this.y *= invNorm;
			this.z *= invNorm;
		}

		/// <summary>
		/// Return a normalized copy of this quaternion.
		/// This quaternion instance is not changed
		/// </summary>
		/// <value>The normalized.</value>
		public Quaternion Normalized {
			get {
				var q = new Quaternion (this);
				q.Normalize ();
				return q;
			}
		}

		public Quaternion Conjugate {
			get {
				return new Quaternion (this.w, -this.x, -this.y, -this.z);
			}
		}

		public Quaternion Inverse {
			get {
				return this.Conjugate / this.NormSquared;
			}
		}

		/// <summary>
		/// Computes the dot product of this product dot another quaternion.
		/// </summary>
		/// <param name="q2">The quaternion to compute the dot product with</param>
		public double Dot(Quaternion other) {
			return this.w * other.w + this.x * other.x + this.y * other.y + this.z * other.z;
		}

		public Quaternion64(double w, double x, double y, double z) {
			this.w = w;
			this.x = x;
			this.y = y;
			this.z = z;
		}

		public Quaternion64(Quaternion64 other) {
			this.w = other.w;
			this.x = other.x;
			this.y = other.y;
			this.z = other.z;
		}


		/// <summary>
		/// Create a new quaternion from an axis-angle
		/// See:
		/// http://www.astro.rug.nl/software/kapteyn/_downloads/attitude.pdf
		/// 6.12 Unit Quaternion ⇐ Axis-Angle
		///              _           _
		/// qa (α, n) := | cos α/2   |
		///              | n sin α/2 |
		///
		/// </summary>
		/// <returns>The axis angle.</returns>
		[System.Obsolete("Semantic Version Opt-Out: this routine has not been finalized yet and may change in breaking ways within minor versions.")]
		public static Quaternion64 FromAxisAngle(double angle, double x, double y, double z) {
			var vNorm = x * x + y * y + z * z;
			var invNorm = 1.0 / Math.Sqrt (vNorm);
			var x2 = x * invNorm;
			var y2 = y * invNorm;
			var z2 = z * invNorm;

			var halfAngle = angle * 0.5;
			var s = Math.Sin(halfAngle);
			var c = Math.Cos(halfAngle);
			return new Quaternion64 (c, x2 * s, y2 * s, z2 * s);
		}

		/// <summary>
		/// Rotate a vector(x, y, z) by the current quaternion instance
		/// p is a pure quaternion(i.e w=0.0)
		/// p' = qpq**-1.0
		/// https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#The_conjugation_operation
		/// </summary>
		/// <param name="x">The x coordinate of the vector.</param>
		/// <param name="y">The y coordinate of the vector.</param>
		/// <param name="z">The z coordinate of the vector.</param>
		[System.Obsolete("Semantic Version Opt-Out: this routine has not been finalized yet and may change in breaking ways within minor versions.")]
		public Quaternion64 Rotate(double x, double y, double z) {
			var q = this.Normalized; //ensure unit quaternion
			var p = new Quaternion64(0.0, x, y, z);
			return q * p * q.Inverse;
		}

		/// <summary>
		/// Concatenate the specified other.
		/// </summary>
		/// <param name="other">Other.</param>
		[System.Obsolete("Semantic Version Opt-Out: this routine has not been finalized yet and may change in breaking ways within minor versions.")]
		public Quaternion64 Concatenate(Quaternion64 other) {
			return other * this;
		}

		public static Quaternion64 operator +(Quaternion64 r, Quaternion64 q) 
		{
			return new Quaternion64(r.w+q.w, r.x+q.x, r.y+q.y, r.z+q.z);
		}

		public static Quaternion64 operator -(Quaternion64 r, Quaternion64 q) 
		{
			return new Quaternion64(r.w-q.w, r.x-q.x, r.y-q.y, r.z-q.z);
		}

		public static Quaternion64 operator *(Quaternion64 r, Quaternion64 q) 
		{
			var w = r.w * q.w - r.x * q.x - r.y * q.y - r.z * q.z;
			var x = r.w * q.x + r.x * q.w - r.y * q.z + r.z * q.y;
			var y = r.w * q.y + r.x * q.z + r.y * q.w - r.z * q.x;
			var z = r.w * q.z - r.x * q.y + r.y * q.x + r.z * q.w;
			return new Quaternion64(w, x, y, z);
		}

		public static Quaternion64 operator /(Quaternion64 r, Quaternion64 q) 
		{
			var d = r.NormSquared;
			var w = (r.w * q.w + r.x * q.x + r.y * q.y + r.z * q.z) / d;
			var x = (r.w * q.x - r.x * q.w - r.y * q.z + r.z * q.y) / d;
			var y = (r.w * q.y + r.x * q.z - r.y * q.w - r.z * q.x) / d;
			var z = (r.w * q.z - r.x * q.y + r.y * q.x - r.z * q.w) / d;
			return new Quaternion64(w, x, y, z);
		}

		public static Quaternion64 operator /(Quaternion64 r, double q) 
		{
			return new Quaternion64(r.w / q, r.x / q, r.y / q, r.z / q);
		}
	}

	public static class InclinationConverter
	{
		public static Quaternion64 eulerToQuaternion(double pitch, double roll, double yaw) {
			//http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/
			var degToRad = Math.PI / 180;
			var c1 = Math.Cos (degToRad * pitch / 2.0);
			var c2 = Math.Cos (degToRad * roll / 2.0);
			var c3 = Math.Cos (degToRad * yaw / 2.0);
			var s1 = Math.Sin (degToRad * pitch / 2.0);
			var s2 = Math.Sin (degToRad * roll / 2.0);
			var s3 = Math.Sin (degToRad * yaw / 2.0);

			var w = c1 * c2 * c3 - s1 * s2 * s3;
			var x = s1 * s2 * c3 + c1 * c2 * s3;
			var y = s1 * c2 * c3 + c1 * s2 * s3;
			var	z = c1 * s2 * c3 - s1 * c2 * s3;

			return new Quaternion(w, x, y, z);
		}
	}
}