public void NaNEquality()
{
Quaternion nanQuat = new Quaternion(float.NaN, float.NaN, float.NaN, float.NaN);
Assert.IsFalse(nanQuat == nanQuat);
Assert.IsTrue(nanQuat != nanQuat);
Assert.IsTrue(nanQuat.Equals(nanQuat));
}
public static void Add (ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)
{
#if SIMD
result = new Quaternion (quaternion1.v4 + quaternion2.v4);
#else
result = new Quaternion();
result.X = quaternion1.X + quaternion2.X;
result.Y = quaternion1.Y + quaternion2.Y;
result.Z = quaternion1.Z + quaternion2.Z;
result.W = quaternion1.W + quaternion2.W;
#endif
}
public static void CreateFromAxisAngle (ref Vector3 axis, float angle, out Quaternion result)
{
Vector3 vec = axis;
vec.Normalize ();
float ang = angle * 0.5f;
Vector3.Multiply (ref vec, (float) System.Math.Sin (ang), out vec);
result = new Quaternion (vec, (float) System.Math.Cos (ang));
}
public static void Slerp (ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result)
{
float dot;
Dot (ref quaternion1, ref quaternion2, out dot);
Quaternion q3;
if (dot < 0.0f) {
dot = -dot;
Negate (ref quaternion2, out q3);
}
else {
q3 = quaternion2;
}
if (dot < 0.999999f) {
float angle = (float) System.Math.Acos (dot);
float sin1 = (float) System.Math.Sin (angle * (1.0f - amount));
float sin2 = (float) System.Math.Sin (angle * amount);
float sin3 = (float) System.Math.Sin (angle);
Quaternion q1;
Multiply (ref quaternion1, sin1, out q1);
Quaternion q2;
Multiply (ref q3, sin2, out q2);
Quaternion q4;
Add (ref q1, ref q2, out q4);
Divide (ref q4, sin3, out result);
}
else {
Lerp (ref quaternion1, ref q3, amount, out result);
}
}
public static void Normalize (ref Quaternion quaternion, out Quaternion result)
{
// TODO: SIMD optimization
Multiply (ref quaternion, 1.0f / quaternion.Length (), out result);
}
public static void Lerp (ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result)
{
Quaternion q1;
Multiply (ref quaternion1, 1.0f - amount, out q1);
Quaternion q2;
Multiply (ref quaternion2, amount, out q2);
Quaternion q1q2;
Add (ref q1, ref q2, out q1q2);
Normalize (ref q1q2, out result);
}
public static Quaternion Inverse (Quaternion quaternion)
{
Inverse (ref quaternion, out quaternion);
return quaternion;
}
public static float Dot (Quaternion quaternion1, Quaternion quaternion2)
{
float result;
Dot (ref quaternion1, ref quaternion2, out result);
return result;
}
public static Quaternion Divide (Quaternion quaternion1, Quaternion quaternion2)
{
Quaternion result;
Divide (ref quaternion1, ref quaternion2, out result);
return result;
}
public static void Multiply (ref Quaternion quaternion1, float scaleFactor, out Quaternion result)
{
#if SIMD
result.v4 = quaternion1.v4 * new Vector4f (scaleFactor);
#else
result.x = quaternion1.x * scaleFactor;
result.y = quaternion1.y * scaleFactor;
result.z = quaternion1.z * scaleFactor;
result.w = quaternion1.w * scaleFactor;
#endif
}
public static Quaternion Multiply (Quaternion quaternion1, float scaleFactor)
{
#if SIMD
return new Quaternion (quaternion1.v4 * new Vector4f (scaleFactor));
#else
return new Quaternion (quaternion1.x * scaleFactor, quaternion1.y * scaleFactor,
quaternion1.z * scaleFactor, quaternion1.w * scaleFactor);
#endif
}
public static void Multiply (ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)
{
// TODO: SIMD optimization
result = new Quaternion();
result.X = quaternion1.W * quaternion2.X + quaternion1.X * quaternion2.W + quaternion1.Y * quaternion2.Z - quaternion1.Z * quaternion2.Y;
result.Y = quaternion1.W * quaternion2.Y - quaternion1.X * quaternion2.Z + quaternion1.Y * quaternion2.W + quaternion1.Z * quaternion2.X;
result.Z = quaternion1.W * quaternion2.Z + quaternion1.X * quaternion2.Y - quaternion1.Y * quaternion2.X + quaternion1.Z * quaternion2.W;
result.W = quaternion1.W * quaternion2.W - quaternion1.X * quaternion2.X - quaternion1.Y * quaternion2.Y - quaternion1.Z * quaternion2.Z;
}
public static Quaternion Multiply (Quaternion quaternion1, Quaternion quaternion2)
{
// TODO: SIMD optimization
return new Quaternion (
quaternion1.W * quaternion2.X + quaternion1.X * quaternion2.W + quaternion1.Y * quaternion2.Z - quaternion1.Z * quaternion2.Y,
quaternion1.W * quaternion2.Y - quaternion1.X * quaternion2.Z + quaternion1.Y * quaternion2.W + quaternion1.Z * quaternion2.X,
quaternion1.W * quaternion2.Z + quaternion1.X * quaternion2.Y - quaternion1.Y * quaternion2.X + quaternion1.Z * quaternion2.W,
quaternion1.W * quaternion2.W - quaternion1.X * quaternion2.X - quaternion1.Y * quaternion2.Y - quaternion1.Z * quaternion2.Z);
}
public static void Subtract (ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)
{
#if SIMD
result = new Quaternion (quaternion1.v4 - quaternion2.v4);
#else
result = new Quaternion();
result.X = quaternion1.X - quaternion2.X;
result.Y = quaternion1.Y - quaternion2.Y;
result.Z = quaternion1.Z - quaternion2.Z;
result.W = quaternion1.W - quaternion2.W;
#endif
}
public static Quaternion Subtract (Quaternion quaternion1, Quaternion quaternion2)
{
#if SIMD
return new Quaternion (quaternion1.v4 - quaternion2.v4);
#else
return new Quaternion (quaternion1.X - quaternion2.X, quaternion1.Y - quaternion2.Y,
quaternion1.Z - quaternion2.Z, quaternion1.W - quaternion2.W);
#endif
}
public static Quaternion Conjugate (Quaternion value)
{
Conjugate (ref value, out value);
return value;
}
public static void Conjugate (ref Quaternion value, out Quaternion result)
{
result = new Quaternion();
result.X = -value.X;
result.Y = - value.Y;
result.Z = - value.Z;
result.W = value.W;
}
public static void Divide (ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)
{
Quaternion inv;
Inverse (ref quaternion2, out inv);
Multiply (ref quaternion1, ref inv, out result);
}
public static void Dot (ref Quaternion quaternion1, ref Quaternion quaternion2, out float result)
{
#if SIMD
//NOTE: shuffle->add->shuffle->add is faster than horizontal-add->horizontal-add
Vector4f r0 = quaternion2.v4 * quaternion1.v4;
// r0 = xX yY zZ wW
Vector4f r1 = r0.Shuffle (ShuffleSel.Swap);
//r1 = zZ wW xX yY
r0 = r0 + r1;
//r0 = xX+zZ yY+wW xX+zZ yY+wW
r1 = r0.Shuffle (ShuffleSel.RotateLeft);
//r1 = yY+wW xX+zZ yY+wW xX+zZ
r0 = r0 + r1;
//r0 = xX+yY+zZ+wW xX+yY+zZ+wW xX+yY+zZ+wW xX+yY+zZ+wW
result = r0.Sqrt ().X;
#else
result = (quaternion1.X * quaternion2.X) + (quaternion1.Y * quaternion2.Y) +
(quaternion1.Z * quaternion2.Z) + (quaternion1.W * quaternion2.W);
#endif
}
public static Quaternion Divide (Quaternion quaternion1, float scaleFactor)
{
#if SIMD
return new Quaternion (quaternion1.v4 / new Vector4f (scaleFactor));
#else
return new Quaternion (quaternion1.x / scaleFactor, quaternion1.y / scaleFactor,
quaternion1.z / scaleFactor, quaternion1.w / scaleFactor);
#endif
}
public static void Inverse (ref Quaternion quaternion, out Quaternion result)
{
// http://www.ncsa.illinois.edu/~kindr/emtc/quaternions/quaternion.c++
Quaternion conj = new Quaternion (quaternion.X, quaternion.Y, quaternion.Z, quaternion.W);
conj.Conjugate ();
result = conj * (1.0f / quaternion.LengthSquared ());
}
public static void Divide (ref Quaternion quaternion1, float scaleFactor, out Quaternion result)
{
#if SIMD
result.v4 = quaternion1.v4 / new Vector4f (scaleFactor);
#else
result.x = quaternion1.x / scaleFactor;
result.y = quaternion1.y / scaleFactor;
result.z = quaternion1.z / scaleFactor;
result.w = quaternion1.w / scaleFactor;
#endif
}
public static Quaternion Normalize (Quaternion quaternion)
{
Normalize (ref quaternion, out quaternion);
return quaternion;
}
public static Quaternion Negate (Quaternion quaternion)
{
#if SIMD
return new Quaternion (quaternion.v4 ^ new Vector4f (-0.0f));
#else
return new Quaternion (- quaternion.x, - quaternion.y, - quaternion.z, - quaternion.w);
#endif
}
public static Quaternion Slerp (Quaternion quaternion1, Quaternion quaternion2, float amount)
{
Quaternion result;
Slerp (ref quaternion1, ref quaternion2, amount, out result);
return result;
}
public static void Negate (ref Quaternion quaternion, out Quaternion result)
{
#if SIMD
result.v4 = quaternion.v4 ^ new Vector4f (-0.0f);
#else
result.x = - quaternion.x;
result.y = - quaternion.y;
result.z = - quaternion.z;
result.w = - quaternion.w;
#endif
}
public bool Equals (Quaternion other)
{
return x.Equals(other.x) && y.Equals(other.y) && z.Equals(other.z) && w.Equals(other.w);
}
public static Quaternion Concatenate (Quaternion value1, Quaternion value2)
{
Quaternion result;
Concatenate (ref value1, ref value2, out result);
return result;
}
public static void Concatenate (ref Quaternion value1, ref Quaternion value2, out Quaternion result)
{
Multiply (ref value1, ref value2, out result);
}
public static Quaternion Add (Quaternion quaternion1, Quaternion quaternion2)
{
#if SIMD
return new Quaternion (quaternion1.v4 + quaternion2.v4);
#else
return new Quaternion (quaternion1.X + quaternion2.X, quaternion1.Y + quaternion2.Y,
quaternion1.Z + quaternion2.Z, quaternion1.W + quaternion2.W);
#endif
}
