public override int Add(QuaternionD value)
{
lock (this._root) return this._collection.Add(value);
}
public override void CopyTo(QuaternionD[] array)
{
lock (this._root) this._collection.CopyTo(array);
}
/// <overloads>
/// Copies the <see cref="QuaternionDArrayList"/> or a portion of it to a one-dimensional array.
/// </overloads>
/// <summary>
/// Copies the entire <see cref="QuaternionDArrayList"/> to a one-dimensional <see cref="Array"/>
/// of <see cref="QuaternionD"/> elements, starting at the beginning of the target array.
/// </summary>
/// <param name="array">The one-dimensional <see cref="Array"/> that is the destination of the
/// <see cref="QuaternionD"/> elements copied from the <see cref="QuaternionDArrayList"/>.
/// The <b>Array</b> must have zero-based indexing.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="array"/> is a null reference.</exception>
/// <exception cref="ArgumentException">
/// The number of elements in the source <see cref="QuaternionDArrayList"/> is greater
/// than the available space in the destination <paramref name="array"/>.</exception>
/// <remarks>Please refer to <see cref="ArrayList.CopyTo"/> for details.</remarks>
public virtual void CopyTo(QuaternionD[] array)
{
CheckTargetArray(array, 0);
Array.Copy(this._array, array, this._count);
}
public override void AddRange(QuaternionD[] array)
{
throw new NotSupportedException(
"Read-only collections cannot be modified.");
}
public override void Remove(QuaternionD value)
{
this._collection.Remove(value);
}
/// <summary>
/// Adds a <see cref="QuaternionD"/> to the end of the <see cref="QuaternionDArrayList"/>.
/// </summary>
/// <param name="value">The <see cref="QuaternionD"/> object
/// to be added to the end of the <see cref="QuaternionDArrayList"/>.
/// </param>
/// <returns>The <see cref="QuaternionDArrayList"/> index at which the
/// <paramref name="value"/> has been added.</returns>
/// <exception cref="NotSupportedException">
/// <para>The <see cref="QuaternionDArrayList"/> is read-only.</para>
/// <para>-or-</para>
/// <para>The <b>QuaternionDArrayList</b> has a fixed size.</para>
/// <para>-or-</para>
/// <para>The <b>QuaternionDArrayList</b> already contains the specified
/// <paramref name="value"/>, and the <b>QuaternionDArrayList</b>
/// ensures that all elements are unique.</para></exception>
/// <remarks>Please refer to <see cref="ArrayList.Add"/> for details.</remarks>
public virtual int Add(QuaternionD value)
{
if (this._count == this._array.Length)
EnsureCapacity(this._count + 1);
++this._version;
this._array[this._count] = value;
return this._count++;
}
/// <summary>
/// Multiplies a quaternion by a scalar.
/// </summary>
/// <param name="q">A <see cref="QuaternionD"/> instance.</param>
/// <param name="s">A scalar.</param>
/// <returns>A <see cref="QuaternionD"/> instance to hold the result.</returns>
public static QuaternionD operator*(QuaternionD q, double s)
{
return(QuaternionD.Multiply(q, s));
}
public override void CopyTo(QuaternionD[] array)
{
this._collection.CopyTo(array);
}
/// <summary>
/// Subtracts a quaternion from a quaternion.
/// </summary>
/// <param name="a">A <see cref="QuaternionD"/> instance.</param>
/// <param name="b">A <see cref="QuaternionD"/> instance.</param>
/// <returns>A new <see cref="QuaternionD"/> instance containing the difference.</returns>
public static QuaternionD operator-(QuaternionD a, QuaternionD b)
{
return(QuaternionD.Subtract(a, b));
}
/// <summary>
/// Multiplies quaternion <paramref name="a"/> by quaternion <paramref name="b"/>.
/// </summary>
/// <param name="a">A <see cref="QuaternionD"/> instance.</param>
/// <param name="b">A <see cref="QuaternionD"/> instance.</param>
/// <returns>A new <see cref="QuaternionD"/> containing the result.</returns>
public static QuaternionD operator*(QuaternionD a, QuaternionD b)
{
return(QuaternionD.Multiply(a, b));
}
/// <summary>
/// Adds two quaternions.
/// </summary>
/// <param name="a">A <see cref="QuaternionD"/> instance.</param>
/// <param name="b">A <see cref="QuaternionD"/> instance.</param>
/// <returns>A new <see cref="QuaternionD"/> instance containing the sum.</returns>
public static QuaternionD operator+(QuaternionD a, QuaternionD b)
{
return(QuaternionD.Add(a, b));
}
/// <summary>
/// Calculates the dot product of two quaternions.
/// </summary>
/// <param name="a">A <see cref="QuaternionD"/> instance.</param>
/// <param name="b">A <see cref="QuaternionD"/> instance.</param>
/// <returns>The dot product value.</returns>
public static double Dot(QuaternionD a, QuaternionD b)
{
return(a.W * b.W + a.X * b.X + a.Y * b.Y + a.Z * b.Z);
}
/// <summary>
/// Subtracts a quaternion from a quaternion.
/// </summary>
/// <param name="a">A <see cref="QuaternionD"/> instance.</param>
/// <param name="b">A <see cref="QuaternionD"/> instance.</param>
/// <returns>A new <see cref="QuaternionD"/> instance containing the difference.</returns>
public static QuaternionD Subtract(QuaternionD a, QuaternionD b)
{
return(new QuaternionD(a.W - b.W, a.X - b.X, a.Y - b.Y, a.Z - b.Z));
}
public override void Remove(QuaternionD value)
{
lock (this._root) this._collection.Remove(value);
}
/// <summary>
/// Divides a quaternion by a scalar.
/// </summary>
/// <param name="q">A <see cref="QuaternionD"/> instance.</param>
/// <param name="s">A scalar.</param>
/// <returns>A <see cref="QuaternionD"/> instance to hold the result.</returns>
public static QuaternionD operator/(QuaternionD q, double s)
{
return(QuaternionD.Divide(q, s));
}
public override void AddRange(QuaternionD[] array)
{
foreach (QuaternionD value in array)
CheckUnique(value);
this._collection.AddRange(array);
}
/// <summary>
/// Divides a scalar by a quaternion.
/// </summary>
/// <param name="q">A <see cref="QuaternionD"/> instance.</param>
/// <param name="s">A scalar.</param>
/// <returns>A <see cref="QuaternionD"/> instance to hold the result.</returns>
public static QuaternionD operator/(double s, QuaternionD q)
{
return(QuaternionD.Multiply(q, 1 / s));
}
public override int IndexOf(QuaternionD value)
{
return this._collection.IndexOf(value);
}
public void Identity_IsIdentity()
{
var expected = new QuaternionD(0, 0, 0, 1);
Assert.Equal(expected, QuaternionD.Identity);
}
private void CheckUnique(int index, QuaternionD value)
{
int existing = IndexOf(value);
if (existing >= 0 && existing != index)
throw new NotSupportedException(
"Unique collections cannot contain duplicate elements.");
}
public void Sub_Static(QuaternionD left, QuaternionD right, QuaternionD expected)
{
var actual = QuaternionD.Sub(left, right);
Assert.Equal(expected, actual);
}
/// <summary>
/// Searches the entire sorted <see cref="QuaternionDArrayList"/> for an
/// <see cref="QuaternionD"/> element using the default comparer
/// and returns the zero-based index of the element.
/// </summary>
/// <param name="value">The <see cref="QuaternionD"/> object
/// to locate in the <see cref="QuaternionDArrayList"/>.
/// </param>
/// <returns>The zero-based index of <paramref name="value"/> in the sorted
/// <see cref="QuaternionDArrayList"/>, if <paramref name="value"/> is found;
/// otherwise, a negative number, which is the bitwise complement of the index
/// of the next element that is larger than <paramref name="value"/> or, if there
/// is no larger element, the bitwise complement of <see cref="Count"/>.</returns>
/// <exception cref="InvalidOperationException">
/// Neither <paramref name="value"/> nor the elements of the <see cref="QuaternionDArrayList"/>
/// implement the <see cref="IComparable"/> interface.</exception>
/// <remarks>Please refer to <see cref="ArrayList.BinarySearch"/> for details.</remarks>
public virtual int BinarySearch(QuaternionD value)
{
return Array.BinarySearch(this._array, 0, this._count, value);
}
public void Multiply_TwoQuaternions_Static(QuaternionD left, QuaternionD right, QuaternionD expected)
{
var actual = QuaternionD.Multiply(left, right);
Assert.Equal(expected, actual);
}
/// <summary>
/// Adds two quaternions.
/// </summary>
/// <param name="a">A <see cref="QuaternionD"/> instance.</param>
/// <param name="b">A <see cref="QuaternionD"/> instance.</param>
/// <returns>A new <see cref="QuaternionD"/> instance containing the sum.</returns>
public static QuaternionD Add(QuaternionD a, QuaternionD b)
{
return(new QuaternionD(a.W + b.W, a.X + b.X, a.Y + b.Y, a.Z + b.Z));
}
public void Multiply_QuaternionScalar_Staic(QuaternionD quat, double scale, QuaternionD expected)
{
var actual = QuaternionD.Multiply(quat, scale);
Assert.Equal(expected, actual);
}
public override void Remove(QuaternionD value)
{
throw new NotSupportedException(
"Read-only collections cannot be modified.");
}
public void Transform_double4(double4 vec, QuaternionD quat, double4 expected)
{
var actual = QuaternionD.Transform(vec, quat);
Assert.Equal(expected, actual);
}
public override void AddRange(QuaternionD[] array)
{
lock (this._root) this._collection.AddRange(array);
}
public void Sub_Operator(QuaternionD left, QuaternionD right, QuaternionD expected)
{
var actual = left - right;
Assert.Equal(expected, actual);
}
public override void Insert(int index, QuaternionD value)
{
lock (this._root) this._collection.Insert(index, value);
}
public void Multiply_TwoQuaternions_Operator(QuaternionD left, QuaternionD right, QuaternionD expected)
{
var actual = left * right;
Assert.Equal(expected, actual);
}
public override int Add(QuaternionD value)
{
CheckUnique(value);
return this._collection.Add(value);
}
public void Multiply_QuaternionScalar_Operator(QuaternionD quat, double scale, QuaternionD expected)
{
var actual = quat * scale;
Assert.Equal(expected, actual);
}
public override int BinarySearch(QuaternionD value)
{
return this._collection.BinarySearch(value);
}
public void Multiply_ScalarQuaternion_Operator(QuaternionD quat, double scale, QuaternionD expected)
{
var actual = scale * quat;
Assert.Equal(expected, actual);
}
public override void CopyTo(QuaternionD[] array, int arrayIndex)
{
this._collection.CopyTo(array, arrayIndex);
}
/// <summary>
/// Calculates the exponent of a quaternion.
/// </summary>
/// <param name="a">A <see cref="QuaternionD"/> instance.</param>
/// <returns>The quaternion's exponent.</returns>
public QuaternionD Exp(QuaternionD a)
{
QuaternionD result = new QuaternionD(0,0,0,0);
double angle = System.Math.Sqrt(a.X*a.X + a.Y*a.Y + a.Z*a.Z);
double sin = System.Math.Sin(angle);
if (MathFunctions.Abs(sin) > 0)
{
double coeff = angle / sin;
result.X = coeff * a.X;
result.Y = coeff * a.Y;
result.Z = coeff * a.Z;
}
else
{
result.X = a.X;
result.Y = a.Y;
result.Z = a.Z;
}
return result;
}
public override void Insert(int index, QuaternionD value)
{
CheckUnique(value);
this._collection.Insert(index, value);
}
/// <summary>
/// Builds a rotation matrix from a quaternion.
/// </summary>
/// <param name="q">A <see cref="QuaternionF"/> instance.</param>
/// <returns>A <see cref="Matrix4D"/> representing the rotation.</returns>
public static Matrix4D RotationQuaternion(QuaternionD q)
{
// TODO: Implement this.
throw new NotImplementedException();
}
private void CheckUnique(QuaternionD value)
{
if (IndexOf(value) >= 0)
throw new NotSupportedException(
"Unique collections cannot contain duplicate elements.");
}
/// <summary>
/// Returns the zero-based index of the first occurrence of the specified
/// <see cref="QuaternionD"/> in the <see cref="QuaternionDArrayList"/>.
/// </summary>
/// <param name="value">The <see cref="QuaternionD"/> object
/// to locate in the <see cref="QuaternionDArrayList"/>.
/// </param>
/// <returns>
/// The zero-based index of the first occurrence of <paramref name="value"/>
/// in the <see cref="QuaternionDArrayList"/>, if found; otherwise, -1.
/// </returns>
/// <remarks>Please refer to <see cref="ArrayList.IndexOf"/> for details.</remarks>
public virtual int IndexOf(QuaternionD value)
{
return Array.IndexOf(this._array, value, 0, this._count);
}
/// <summary>
/// Initializes a new instance of the <see cref="QuaternionDArrayList"/> class
/// that contains elements copied from the specified <see cref="QuaternionD"/>
/// array and that has the same initial capacity as the number of elements copied.
/// </summary>
/// <param name="array">An <see cref="Array"/> of <see cref="QuaternionD"/>
/// elements that are copied to the new collection.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="array"/> is a null reference.</exception>
/// <remarks>Please refer to <see cref="ArrayList(ICollection)"/> for details.</remarks>
public QuaternionDArrayList(QuaternionD[] array)
{
if (array == null)
throw new ArgumentNullException("array");
this._array = new QuaternionD[array.Length];
AddRange(array);
}
/// <summary>
/// Inserts a <see cref="QuaternionD"/> element into the
/// <see cref="QuaternionDArrayList"/> at the specified index.
/// </summary>
/// <param name="index">The zero-based index at which <paramref name="value"/>
/// should be inserted.</param>
/// <param name="value">The <see cref="QuaternionD"/> object
/// to insert into the <see cref="QuaternionDArrayList"/>.
/// </param>
/// <exception cref="ArgumentOutOfRangeException">
/// <para><paramref name="index"/> is less than zero.</para>
/// <para>-or-</para>
/// <para><paramref name="index"/> is greater than <see cref="Count"/>.</para>
/// </exception>
/// <exception cref="NotSupportedException">
/// <para>The <see cref="QuaternionDArrayList"/> is read-only.</para>
/// <para>-or-</para>
/// <para>The <b>QuaternionDArrayList</b> has a fixed size.</para>
/// <para>-or-</para>
/// <para>The <b>QuaternionDArrayList</b> already contains the specified
/// <paramref name="value"/>, and the <b>QuaternionDArrayList</b>
/// ensures that all elements are unique.</para></exception>
/// <remarks>Please refer to <see cref="ArrayList.Insert"/> for details.</remarks>
public virtual void Insert(int index, QuaternionD value)
{
if (index < 0)
throw new ArgumentOutOfRangeException("index",
index, "Argument cannot be negative.");
if (index > this._count)
throw new ArgumentOutOfRangeException("index",
index, "Argument cannot exceed Count.");
if (this._count == this._array.Length)
EnsureCapacity(this._count + 1);
++this._version;
if (index < this._count)
Array.Copy(this._array, index,
this._array, index + 1, this._count - index);
this._array[index] = value;
++this._count;
}
/// <summary>
/// Adds the elements of a <see cref="QuaternionD"/> array
/// to the end of the <see cref="QuaternionDArrayList"/>.
/// </summary>
/// <param name="array">An <see cref="Array"/> of <see cref="QuaternionD"/> elements
/// that should be added to the end of the <see cref="QuaternionDArrayList"/>.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="array"/> is a null reference.</exception>
/// <exception cref="NotSupportedException">
/// <para>The <see cref="QuaternionDArrayList"/> is read-only.</para>
/// <para>-or-</para>
/// <para>The <b>QuaternionDArrayList</b> has a fixed size.</para>
/// <para>-or-</para>
/// <para>The <b>QuaternionDArrayList</b> already contains one or more elements
/// in the specified <paramref name="array"/>, and the <b>QuaternionDArrayList</b>
/// ensures that all elements are unique.</para></exception>
/// <remarks>Please refer to <see cref="ArrayList.AddRange"/> for details.</remarks>
public virtual void AddRange(QuaternionD[] array)
{
if (array == null)
throw new ArgumentNullException("array");
if (array.Length == 0) return;
if (this._count + array.Length > this._array.Length)
EnsureCapacity(this._count + array.Length);
++this._version;
Array.Copy(array, 0, this._array, this._count, array.Length);
this._count += array.Length;
}
/// <summary>
/// Removes the first occurrence of the specified <see cref="QuaternionD"/>
/// from the <see cref="QuaternionDArrayList"/>.
/// </summary>
/// <param name="value">The <see cref="QuaternionD"/> object
/// to remove from the <see cref="QuaternionDArrayList"/>.
/// </param>
/// <exception cref="NotSupportedException">
/// <para>The <see cref="QuaternionDArrayList"/> is read-only.</para>
/// <para>-or-</para>
/// <para>The <b>QuaternionDArrayList</b> has a fixed size.</para></exception>
/// <remarks>Please refer to <see cref="ArrayList.Remove"/> for details.</remarks>
public virtual void Remove(QuaternionD value)
{
int index = IndexOf(value);
if (index >= 0) RemoveAt(index);
}
/// <summary>
/// Determines whether the <see cref="QuaternionDArrayList"/>
/// contains the specified <see cref="QuaternionD"/> element.
/// </summary>
/// <param name="value">The <see cref="QuaternionD"/> object
/// to locate in the <see cref="QuaternionDArrayList"/>.
/// </param>
/// <returns><c>true</c> if <paramref name="value"/> is found in the
/// <see cref="QuaternionDArrayList"/>; otherwise, <c>false</c>.</returns>
/// <remarks>Please refer to <see cref="ArrayList.Contains"/> for details.</remarks>
public bool Contains(QuaternionD value)
{
return (IndexOf(value) >= 0);
}
/// <summary>
/// Copies the elements of the <see cref="QuaternionDArrayList"/> to a new
/// <see cref="Array"/> of <see cref="QuaternionD"/> elements.
/// </summary>
/// <returns>A one-dimensional <see cref="Array"/> of <see cref="QuaternionD"/>
/// elements containing copies of the elements of the <see cref="QuaternionDArrayList"/>.</returns>
/// <remarks>Please refer to <see cref="ArrayList.ToArray"/> for details.</remarks>
public virtual QuaternionD[] ToArray()
{
QuaternionD[] array = new QuaternionD[this._count];
Array.Copy(this._array, array, this._count);
return array;
}
/// <summary>
/// Copies the entire <see cref="QuaternionDArrayList"/> to a one-dimensional <see cref="Array"/>
/// of <see cref="QuaternionD"/> elements, starting at the specified index of the target array.
/// </summary>
/// <param name="array">The one-dimensional <see cref="Array"/> that is the destination of the
/// <see cref="QuaternionD"/> elements copied from the <see cref="QuaternionDArrayList"/>.
/// The <b>Array</b> must have zero-based indexing.</param>
/// <param name="arrayIndex">The zero-based index in <paramref name="array"/>
/// at which copying begins.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="array"/> is a null reference.</exception>
/// <exception cref="ArgumentOutOfRangeException">
/// <paramref name="arrayIndex"/> is less than zero.</exception>
/// <exception cref="ArgumentException"><para>
/// <paramref name="arrayIndex"/> is equal to or greater than the length of <paramref name="array"/>.
/// </para><para>-or-</para><para>
/// The number of elements in the source <see cref="QuaternionDArrayList"/> is greater than the
/// available space from <paramref name="arrayIndex"/> to the end of the destination
/// <paramref name="array"/>.</para></exception>
/// <remarks>Please refer to <see cref="ArrayList.CopyTo"/> for details.</remarks>
public virtual void CopyTo(QuaternionD[] array, int arrayIndex)
{
CheckTargetArray(array, arrayIndex);
Array.Copy(this._array, 0, array, arrayIndex, this._count);
}
/// <summary>
/// Diagonalizes a given covariance matrix with double precision.
///
/// <para>
/// Credits to: https://github.com/melax/sandbox
/// http://melax.github.io/diag.html
/// A must be a symmetric matrix.
/// returns quaternion q such that its corresponding matrix Q
/// can be used to Diagonalize A
/// Diagonal matrix D = transpose(Q) * A * (Q); thus A == Q * D * QT
/// The directions of Q (cols of Q) are the eigenvectors D's diagonal is the eigenvalues.
/// As per 'col' convention if float3x3 Q = qgetmatrix(q); then Q*v = q*v*conj(q).
/// </para>
/// </summary>
/// <param name="A">The covariance matrix</param>
/// <returns></returns>
public static EigenD Diagonalizer(double4x4 A)
{
const int maxsteps = 512; // certainly wont need that many.
var q = new QuaternionD(0, 0, 0, 1);
var D = double4x4.Identity;
var Q = double4x4.Identity;
for (var i = 0; i < maxsteps; i++)
{
// Q = float4x4.CreateRotation(q); // v*Q == q*v*conj(q)
Q = q.ToRotMat(); // v*Q == q*v*conj(q)
D = Q.Transpose() * A * Q; // A = Q^T*D*Q
var offDiagonal = new double3(D.M23, D.M13, D.M12); // elements not on the diagonal
var om = new double3(System.Math.Abs(offDiagonal.x), System.Math.Abs(offDiagonal.y), System.Math.Abs(offDiagonal.z)); // mag of each offdiag elem
var k = (om.x > om.y && om.x > om.z) ? 0 : (om.y > om.z) ? 1 : 2; // index of largest element of offdiag
var k1 = (k + 1) % 3;
var k2 = (k + 2) % 3;
if (offDiagonal[k].Equals(0.0f))
{
break; // diagonal already
}
var theta = (D[k2, k2] - D[k1, k1]) / (2.0f * offDiagonal[k]);
var sgn = (theta > 0.0f) ? 1.0f : -1.0f;
theta *= sgn; // make it positive
var t = sgn / (theta + ((theta < 1.0e+6f) ? System.Math.Sqrt(theta * theta + 1.0f) : theta)); // sign(T)/(|T|+sqrt(T^2+1))
var c = 1.0f / System.Math.Sqrt(t * t + 1.0f); // c= 1/(t^2+1) , t=s/c
if (c.Equals(1.0f))
{
break; // no room for improvement - reached machine precision.
}
var jr = new QuaternionD(0, 0, 0, 0) // jacobi rotation for this iteration.
{
[k] = (sgn * System.Math.Sqrt((1.0f - c) / 2.0f))
};
// using 1/2 angle identity sin(a/2) = sqrt((1-cos(a))/2)
jr[k] *= -1.0f; // note we want a final result semantic that takes D to A, not A to D
jr.w = System.Math.Sqrt(1.0f - (jr[k] * jr[k]));
if (jr.w.Equals(1.0f))
{
break; // reached limits of floating point precision
}
q *= jr;
q.Normalize();
}
var vectorMat = Q;
return(new EigenD
{
Values = new[]
{
D.M11,
D.M22,
D.M33
},
Vectors = new double3[] { vectorMat.Row0.xyz, vectorMat.Row1.xyz, vectorMat.Row2.xyz }
});
}
