public void Power3()
{
const float θ = 0.4f;
Vector3 v = new Vector3(2.3f, 1.0f, -2.0f);
v.Normalize();
Quaternion q = new Quaternion((float)Math.Cos(θ), (float)Math.Sin(θ) * v);
Quaternion q2 = q;
q2.Power(2);
Assert.IsTrue(Quaternion.AreNumericallyEqual(q * q, q2));
Quaternion q3 = q;
q3.Power(3);
Assert.IsTrue(Quaternion.AreNumericallyEqual(q * q * q, q3));
q2 = q;
q2.Power(-2);
Assert.IsTrue(Quaternion.AreNumericallyEqual(q.Inverse * q.Inverse, q2));
q3 = q;
q3.Power(-3);
Assert.IsTrue(Quaternion.AreNumericallyEqual(q.Inverse * q.Inverse * q.Inverse, q3));
}
public void CycleOffsetTest()
{
// IAnimationValueTraits<T> is used in a cyclic animation to a add the cycle offset in
// each iteration.
var traits = QuaternionTraits.Instance;
var first = (Quaternion)_random.NextQuaternion(); // Animation value of first key frame.
var last = (Quaternion)_random.NextQuaternion(); // Animation value of last key frame.
var cycleOffset = traits.Add(traits.Inverse(first), last);
var cycleOffsetInverse = cycleOffset;
cycleOffsetInverse.Conjugate();
// Cycle offset should be the difference between last and first key frame.
Assert.IsTrue(Quaternion.AreNumericallyEqual((Quaternion)last, (Quaternion)traits.Add(first, cycleOffset)));
Assert.IsTrue(Quaternion.AreNumericallyEqual((Quaternion)last, (Quaternion)(cycleOffset * first)));
// Check multiple cycles (post-loop).
Assert.IsTrue(Quaternion.AreNumericallyEqual((Quaternion)last, (Quaternion)traits.Add(first, traits.Multiply(cycleOffset, 1))));
Assert.IsTrue(Quaternion.AreNumericallyEqual((Quaternion)cycleOffset * (Quaternion)cycleOffset * (Quaternion)last, (Quaternion)traits.Add(first, traits.Multiply(cycleOffset, 3))));
// Check multiple cycles (pre-loop).
Assert.IsTrue(Quaternion.AreNumericallyEqual((Quaternion)first, (Quaternion)traits.Add(last, traits.Multiply(cycleOffset, -1))));
Assert.IsTrue(Quaternion.AreNumericallyEqual((Quaternion)cycleOffsetInverse * (Quaternion)cycleOffsetInverse * (Quaternion)first, (Quaternion)traits.Add(last, traits.Multiply(cycleOffset, -3))));
}
public void InterpolateTest()
{
SrtTransform a = new SrtTransform(new Vector3(1, 2, 3), new Quaternion(1, 2, 3, 4).Normalized, new Vector3(4, 5, 6));
SrtTransform b = a;
var c = SrtTransform.Interpolate(a, b, 0.5f);
Assert.AreEqual(a, c);
b = new SrtTransform(new Vector3(7, 9, 8), new Quaternion(6, 6, 4, 2).Normalized, new Vector3(-2, 4, -9));
c = SrtTransform.Interpolate(a, b, 0);
Assert.AreEqual(a, c);
c = SrtTransform.Interpolate(a, b, 1);
Assert.AreEqual(b, c);
c = SrtTransform.Interpolate(a, b, 0.3f);
Assert.AreEqual(a.Translation * 0.7f + b.Translation * 0.3f, c.Translation);
Assert.AreEqual(a.Scale * 0.7f + b.Scale * 0.3f, c.Scale);
Assert.IsTrue(Quaternion.AreNumericallyEqual(
new Quaternion(
a.Rotation.W * 0.7f + b.Rotation.W * 0.3f,
a.Rotation.V * 0.7f + b.Rotation.V * 0.3f).Normalized,
c.Rotation));
}
public void DivisionScalar()
{
float s = 123.456f;
Quaternion q = new Quaternion(1.0f, 2.0f, 3.0f, 4.0f);
Quaternion expectedResult = new Quaternion(1.0f / s, 2.0f / s, 3.0f / s, 4.0f / s);
Quaternion result = Quaternion.Divide(q, s);
Assert.IsTrue(Quaternion.AreNumericallyEqual(expectedResult, result));
}
public void XnaQuaternionMultiplication()
{
Quaternion q1 = _random.NextQuaternion();
Quaternion q2 = _random.NextQuaternion();
var q1Xna = (Quaternion)q1;
var q2Xna = (Quaternion)q2;
Assert.IsTrue(Quaternion.AreNumericallyEqual(q1 * q2, (Quaternion)(q1Xna * q2Xna)));
Assert.IsTrue(Quaternion.AreNumericallyEqual(q2 * q1, (Quaternion)(q2Xna * q1Xna)));
}
public void FromMatrixWithZeroTrace()
{
Quaternion q;
Matrix m = new Matrix(0, 1, 0,
0, 0, 1,
1, 0, 0);
q = Quaternion.CreateFromRotationMatrix(m);
Assert.IsTrue(Quaternion.AreNumericallyEqual(new Quaternion(-0.5f, 0.5f, 0.5f, 0.5f), q));
}
public void InterpolationTest()
{
var traits = QuaternionTraits.Instance;
var value0 = _random.NextQuaternion();
var value1 = _random.NextQuaternion();
Assert.IsTrue(Quaternion.AreNumericallyEqual(value0, traits.Interpolate(value0, value1, 0.0f)));
Assert.IsTrue(Quaternion.AreNumericallyEqual(value1, traits.Interpolate(value0, value1, 1.0f)));
Assert.IsTrue(Quaternion.AreNumericallyEqual(InterpolationHelper.Lerp(value0, value1, 0.75f), traits.Interpolate(value0, value1, 0.75f)));
}
public void Angle()
{
Vector3 axis = new Vector3(1.0f, 2.0f, 3.0f);
Quaternion q = Quaternion.CreateFromRotationMatrix(axis, 0.4f);
Assert.IsTrue(Numeric.AreEqual(0.4f, q.Angle));
q.Angle = 0.9f;
Assert.IsTrue(Quaternion.AreNumericallyEqual(q, Quaternion.CreateFromRotationMatrix(axis, 0.9f)));
Assert.AreEqual(0, new Quaternion(1.000001f, 0, 0, 0).Angle);
}
public void AreEqualWithEpsilon()
{
float epsilon = 0.001f;
Quaternion q1 = new Quaternion(1.0f, 2.0f, 3.0f, 4.0f);
Quaternion q2 = new Quaternion(1.002f, 2.002f, 3.002f, 4.002f);
Quaternion q3 = new Quaternion(1.0001f, 2.0001f, 3.0001f, 4.0001f);
Assert.IsTrue(Quaternion.AreNumericallyEqual(q1, q1, epsilon));
Assert.IsFalse(Quaternion.AreNumericallyEqual(q1, q2, epsilon));
Assert.IsTrue(Quaternion.AreNumericallyEqual(q1, q3, epsilon));
}
public void AnimateUsingDefaults()
{
var defaultSource = _random.NextQuaternion();
var defaultTarget = _random.NextQuaternion();
var animation = new QuaternionFromToByAnimation();
animation.From = null;
animation.To = null;
animation.By = null;
Assert.AreEqual(defaultSource, animation.GetValue(TimeSpan.FromSeconds(0.0), defaultSource, defaultTarget));
Assert.IsTrue(Quaternion.AreNumericallyEqual(InterpolationHelper.Lerp(defaultSource, defaultTarget, 0.75f), animation.GetValue(TimeSpan.FromSeconds(0.75), defaultSource, defaultTarget)));
Assert.AreEqual(defaultTarget, animation.GetValue(TimeSpan.FromSeconds(1.0), defaultSource, defaultTarget));
}
public void MultiplyTest()
{
var traits = QuaternionTraits.Instance;
var value = _random.NextQuaternion();
Assert.IsTrue(Quaternion.AreNumericallyEqual(Quaternion.Identity, traits.Multiply(value, 0)));
Assert.IsTrue(Quaternion.AreNumericallyEqual(value, traits.Multiply(value, 1)));
Assert.IsTrue(Quaternion.AreNumericallyEqual(value * value, traits.Multiply(value, 2)));
Assert.IsTrue(Quaternion.AreNumericallyEqual(value * value * value, traits.Multiply(value, 3)));
Assert.IsTrue(Quaternion.AreNumericallyEqual(value.Inverse, traits.Multiply(value, -1)));
Assert.IsTrue(Quaternion.AreNumericallyEqual(value.Inverse * value.Inverse, traits.Multiply(value, -2)));
Assert.IsTrue(Quaternion.AreNumericallyEqual(value.Inverse * value.Inverse * value.Inverse, traits.Multiply(value, -3)));
}
public void Power()
{
const float θ = 0.4f;
const float t = -1.2f;
Vector3 v = new Vector3(2.3f, 1.0f, -2.0f);
v.Normalize();
Quaternion q = new Quaternion((float)Math.Cos(θ), (float)Math.Sin(θ) * v);
Quaternion power = Quaternion.Power(q, t);
Quaternion expected = new Quaternion((float)Math.Cos(t * θ), (float)Math.Sin(t * θ) * v);
Assert.IsTrue(Quaternion.AreNumericallyEqual(expected, power));
}
public void ShouldIgnoreByIfToIsSet()
{
var defaultSource = _random.NextQuaternion();
var defaultTarget = _random.NextQuaternion();
var to = _random.NextQuaternion();
var by = _random.NextQuaternion();
var animation = new QuaternionFromToByAnimation();
animation.From = null;
animation.To = to;
animation.By = by;
Assert.AreEqual(defaultSource, animation.GetValue(TimeSpan.FromSeconds(0.0), defaultSource, defaultTarget));
Assert.IsTrue(Quaternion.AreNumericallyEqual(InterpolationHelper.Lerp(defaultSource, to, 0.75f), animation.GetValue(TimeSpan.FromSeconds(0.75), defaultSource, defaultTarget)));
Assert.AreEqual(to, animation.GetValue(TimeSpan.FromSeconds(1.0), defaultSource, defaultTarget));
}
public void MultiplyTest()
{
var traits = QuaternionTraits.Instance;
var value = (Quaternion)_random.NextQuaternion();
Quaternion valueInverse = value;
valueInverse.Conjugate();
Assert.IsTrue(Quaternion.AreNumericallyEqual((Quaternion)Quaternion.Identity, (Quaternion)traits.Multiply(value, 0)));
Assert.IsTrue(Quaternion.AreNumericallyEqual((Quaternion)value, (Quaternion)traits.Multiply(value, 1)));
Assert.IsTrue(Quaternion.AreNumericallyEqual((Quaternion)(value * value), (Quaternion)traits.Multiply(value, 2)));
Assert.IsTrue(Quaternion.AreNumericallyEqual((Quaternion)(value * value * value), (Quaternion)traits.Multiply(value, 3)));
Assert.IsTrue(Quaternion.AreNumericallyEqual((Quaternion)valueInverse, (Quaternion)traits.Multiply(value, -1)));
Assert.IsTrue(Quaternion.AreNumericallyEqual((Quaternion)valueInverse * (Quaternion)valueInverse, (Quaternion)traits.Multiply(value, -2)));
Assert.IsTrue(Quaternion.AreNumericallyEqual((Quaternion)valueInverse * (Quaternion)valueInverse * (Quaternion)valueInverse, (Quaternion)traits.Multiply(value, -3)));
}
public void FromByTest()
{
// IAnimationValueTraits<T> is used in a from-by animation to a add a relative offset to
// the start value.
var traits = QuaternionTraits.Instance;
var from = _random.NextQuaternion();
var by = _random.NextQuaternion();
var to = traits.Add(from, by);
Assert.IsTrue(Quaternion.AreNumericallyEqual(by * from, to));
Assert.IsTrue(Quaternion.AreNumericallyEqual(from, traits.Add(to, traits.Inverse(by))));
Assert.IsTrue(Quaternion.AreNumericallyEqual(by, traits.Add(traits.Inverse(from), to)));
}
public void AreEqual()
{
float originalEpsilon = Numeric.EpsilonF;
Numeric.EpsilonF = 1e-8f;
Quaternion q1 = new Quaternion(1.0f, 2.0f, 3.0f, 4.0f);
Quaternion q2 = new Quaternion(1.000001f, 2.000001f, 3.000001f, 4.000001f);
Quaternion q3 = new Quaternion(1.00000001f, 2.00000001f, 3.00000001f, 4.00000001f);
Assert.IsTrue(Quaternion.AreNumericallyEqual(q1, q1));
Assert.IsFalse(Quaternion.AreNumericallyEqual(q1, q2));
Assert.IsTrue(Quaternion.AreNumericallyEqual(q1, q3));
Numeric.EpsilonF = originalEpsilon;
}
public void AnimateFromTo()
{
var defaultSource = _random.NextQuaternion();
var defaultTarget = _random.NextQuaternion();
var from = _random.NextQuaternion();
var to = _random.NextQuaternion();
var by = _random.NextQuaternion();
var animation = new QuaternionFromToByAnimation();
animation.From = from;
animation.To = to;
animation.By = null;
Assert.AreEqual(from, animation.GetValue(TimeSpan.FromSeconds(0.0), defaultSource, defaultTarget));
Assert.IsTrue(Quaternion.AreNumericallyEqual(InterpolationHelper.Lerp(from, to, 0.75f), animation.GetValue(TimeSpan.FromSeconds(0.75), defaultSource, defaultTarget)));
Assert.AreEqual(to, animation.GetValue(TimeSpan.FromSeconds(1.0), defaultSource, defaultTarget));
}
public void SlerpSinglePrecision()
{
// Warning: The not all results are not verified
Quaternion q1 = new Quaternion(1.0f, 2.0f, 3.0f, 4.0f).Normalized;
Quaternion q2 = new Quaternion(2.0f, 4.0f, 6.0f, 8.0f).Normalized;
Quaternion slerp = InterpolationHelper.Slerp(q1, q2, 0.75f);
Assert.IsTrue(slerp.IsNumericallyNormalized);
slerp = InterpolationHelper.Slerp(q1, q2, 0);
Assert.IsTrue(slerp.IsNumericallyNormalized);
Assert.IsTrue(Quaternion.AreNumericallyEqual(q1, slerp));
slerp = InterpolationHelper.Slerp(q1, q2, 1);
Assert.IsTrue(slerp.IsNumericallyNormalized);
Assert.IsTrue(Quaternion.AreNumericallyEqual(q2, slerp));
}
public void Interpolate()
{
Pose p1 = new Pose(new Vector3(1, 2, 3), Quaternion.CreateRotationY(0.3f));
Pose p2 = new Pose(new Vector3(-4, 5, -6), Quaternion.CreateRotationZ(-0.1f));
Assert.IsTrue(Vector3.AreNumericallyEqual(p1.Position, Pose.Interpolate(p1, p2, 0).Position));
Assert.IsTrue(Matrix.AreNumericallyEqual(p1.Orientation, Pose.Interpolate(p1, p2, 0).Orientation));
Assert.IsTrue(Vector3.AreNumericallyEqual(p2.Position, Pose.Interpolate(p1, p2, 1).Position));
Assert.IsTrue(Matrix.AreNumericallyEqual(p2.Orientation, Pose.Interpolate(p1, p2, 1).Orientation));
Assert.IsTrue(Vector3.AreNumericallyEqual(InterpolationHelper.Lerp(p1.Position, p2.Position, 0.3f), Pose.Interpolate(p1, p2, 0.3f).Position));
Assert.IsTrue(
Quaternion.AreNumericallyEqual(
InterpolationHelper.Lerp(Quaternion.CreateFromRotationMatrix(p1.Orientation), Quaternion.CreateFromRotationMatrix(p2.Orientation), 0.3f),
Quaternion.CreateFromRotationMatrix(Pose.Interpolate(p1, p2, 0.3f).Orientation)));
}
public void LerpQuaternion()
{
// Warning: The not all results are not verified
Quaternion q1 = new Quaternion(1.0f, 2.0f, 3.0f, 4.0f).Normalized;
Quaternion q2 = new Quaternion(2.0f, 4.0f, 6.0f, 8.0f).Normalized;
Quaternion lerp = InterpolationHelper.Lerp(q1, q2, 0.75f);
Assert.IsTrue(lerp.IsNumericallyNormalized);
lerp = InterpolationHelper.Lerp(q1, q2, 0);
Assert.IsTrue(Quaternion.AreNumericallyEqual(q1, lerp));
lerp = InterpolationHelper.Lerp(q1, q2, 1);
Assert.IsTrue(Quaternion.AreNumericallyEqual(q2, lerp));
q1 = Quaternion.Identity;
q2 = Quaternion.CreateFromRotationMatrix(Vector3.UnitZ, (float)Math.PI / 2);
lerp = InterpolationHelper.Lerp(q1, q2, 0.5f);
Vector3 v = lerp.Rotate(Vector3.UnitX);
Vector3 result = new Vector3(1.0f, 1.0f, 0.0f).Normalized;
Assert.IsTrue(Vector3.AreNumericallyEqual(result, v));
q1 = Quaternion.Identity;
q2 = Quaternion.CreateFromRotationMatrix(Vector3.UnitY, (float)Math.PI / 2);
lerp = InterpolationHelper.Lerp(q1, q2, 0.5f);
v = lerp.Rotate(Vector3.UnitZ);
result = new Vector3(1.0f, 0.0f, 1.0f).Normalized;
Assert.IsTrue(Vector3.AreNumericallyEqual(result, v));
q1 = Quaternion.Identity;
q2 = Quaternion.CreateFromRotationMatrix(Vector3.UnitX, (float)Math.PI / 2);
lerp = InterpolationHelper.Lerp(q1, q2, 0.5f);
v = lerp.Rotate(Vector3.UnitY);
result = new Vector3(0.0f, 1.0f, 1.0f).Normalized;
Assert.IsTrue(Vector3.AreNumericallyEqual(result, v));
q1 = new Quaternion(-1.0f, 0.0f, 0.0f, 0.0f);
q2 = Quaternion.CreateFromRotationMatrix(-Vector3.UnitZ, (float)-Math.PI / 2);
lerp = InterpolationHelper.Lerp(q1, q2, 0.5f);
v = lerp.Rotate(Vector3.UnitX);
result = new Vector3(1.0f, 1.0f, 0.0f).Normalized;
Assert.IsTrue(Vector3.AreNumericallyEqual(result, v));
}
public void FromToMatrixTest()
{
var t = new Vector3(1, 2, 3);
var r = new Quaternion(1, 2, 3, 4).Normalized;
var s = new Vector3(2, 7, 9);
var m = Matrix.CreateTranslation(t) * Matrix.CreateRotation(r) * Matrix.CreateScale(s);
var srt = SrtTransform.FromMatrix(m);
Assert.IsTrue(Vector3.AreNumericallyEqual(t, srt.Translation));
Assert.IsTrue(Quaternion.AreNumericallyEqual(r, srt.Rotation));
Assert.IsTrue(Vector3.AreNumericallyEqual(s, srt.Scale));
// XNA:
srt = SrtTransform.FromMatrix((Matrix)m);
Assert.IsTrue(Vector3.AreNumericallyEqual(t, srt.Translation));
Assert.IsTrue(Quaternion.AreNumericallyEqual(r, srt.Rotation));
Assert.IsTrue(Vector3.AreNumericallyEqual(s, srt.Scale));
// With negative scale, the decomposition is not unique (many possible combinations of
// axis mirroring + rotation).
t = new Vector3(1, 2, 3);
r = new Quaternion(1, 2, 3, 4).Normalized;
s = new Vector3(2, -7, 9);
m = Matrix.CreateTranslation(t) * Matrix.CreateRotation(r) * Matrix.CreateScale(s);
srt = SrtTransform.FromMatrix(m);
var m2 = (Matrix)srt;
Assert.IsTrue(Matrix.AreNumericallyEqual(m, m2));
m2 = srt.ToMatrix();
Assert.IsTrue(Matrix.AreNumericallyEqual(m, m2));
m2 = srt;
Assert.IsTrue(Matrix.AreNumericallyEqual(m, m2));
Matrix mXna = srt.ToXna();
Assert.IsTrue(Matrix.AreNumericallyEqual(m, (Matrix)mXna));
mXna = srt;
Assert.IsTrue(Matrix.AreNumericallyEqual(m, (Matrix)mXna));
}
public void Inverse()
{
Quaternion identity = Quaternion.Identity;
Quaternion inverseIdentity = identity.Inverse;
Assert.AreEqual(inverseIdentity, identity);
float angle = 0.4f;
Vector3 axis = new Vector3(1.0f, 1.0f, 1.0f);
axis.Normalize();
Quaternion q = Quaternion.CreateFromRotationMatrix(axis, angle);
Quaternion inverse = q.Inverse;
Assert.IsTrue(Vector3.AreNumericallyEqual(-axis, inverse.Axis));
q = new Quaternion(1, 2, 3, 4);
inverse = q.Inverse;
Assert.IsTrue(Quaternion.AreNumericallyEqual(Quaternion.Identity, inverse * q));
}
public void ConstructorTest()
{
var rotationQ = new Quaternion(1, 2, 3, 4).Normalized;
var srt = new SrtTransform(rotationQ.ToRotationMatrix33());
Assert.AreEqual(Vector3.One, srt.Scale);
Assert.IsTrue(Quaternion.AreNumericallyEqual(rotationQ, srt.Rotation));
Assert.AreEqual(Vector3.Zero, srt.Translation);
srt = new SrtTransform(rotationQ);
Assert.AreEqual(Vector3.One, srt.Scale);
Assert.IsTrue(Quaternion.AreNumericallyEqual(rotationQ, srt.Rotation));
Assert.AreEqual(Vector3.Zero, srt.Translation);
srt = new SrtTransform(new Vector3(-1, 2, -3), rotationQ.ToRotationMatrix33(), new Vector3(10, 9, -8));
Assert.AreEqual(new Vector3(-1, 2, -3), srt.Scale);
Assert.IsTrue(Quaternion.AreNumericallyEqual(rotationQ, srt.Rotation));
Assert.AreEqual(new Vector3(10, 9, -8), srt.Translation);
}
public void SquadSinglePrecision()
{
Quaternion q0 = Quaternion.CreateFromRotationMatrix(new Vector3(1, 1, 1), 0.3f);
Quaternion q1 = Quaternion.CreateFromRotationMatrix(new Vector3(1, 0, 1), 0.4f);
Quaternion q2 = Quaternion.CreateFromRotationMatrix(new Vector3(1, 0, -1), -0.6f);
Quaternion q3 = Quaternion.CreateFromRotationMatrix(new Vector3(0, 1, 1), 0.2f);
Quaternion q, a, b, p;
Quaternion expected;
InterpolationHelper.SquadSetup(q0, q1, q2, q3, out q, out a, out b, out p);
// t = 0
Quaternion result = InterpolationHelper.Squad(q, a, b, p, 0.0f);
Assert.IsTrue(Quaternion.AreNumericallyEqual(q1, result));
// t = 1.0f
result = InterpolationHelper.Squad(q, a, b, p, 1.0f);
Assert.IsTrue(Quaternion.AreNumericallyEqual(q2, result));
// Check series (just for debugging)
Quaternion r1, r2, r3, r4, r5, r6, r7, r8, r9;
r1 = InterpolationHelper.Squad(q, a, b, p, 0.1f);
r2 = InterpolationHelper.Squad(q, a, b, p, 0.2f);
r3 = InterpolationHelper.Squad(q, a, b, p, 0.3f);
r4 = InterpolationHelper.Squad(q, a, b, p, 0.4f);
r5 = InterpolationHelper.Squad(q, a, b, p, 0.5f);
r6 = InterpolationHelper.Squad(q, a, b, p, 0.6f);
r7 = InterpolationHelper.Squad(q, a, b, p, 0.7f);
r8 = InterpolationHelper.Squad(q, a, b, p, 0.8f);
r9 = InterpolationHelper.Squad(q, a, b, p, 0.9f);
// q0 = q1, q2 = q3
InterpolationHelper.SquadSetup(q1, q1, q2, q2, out q, out a, out b, out p);
result = InterpolationHelper.Squad(q, a, b, p, 0.5f);
expected = InterpolationHelper.Slerp(q1, q2, 0.5f);
Assert.IsTrue(Quaternion.AreNumericallyEqual(expected, result));
}
public void BlendTest()
{
var traits = SrtTransformTraits.Instance;
var value0 = NextRandomValue();
var value1 = NextRandomValue();
var value2 = NextRandomValue();
var w0 = 0.3f;
var w1 = 0.4f;
var w2 = 1 - w0 - w1;
SrtTransform result = new SrtTransform();
traits.BeginBlend(ref result);
traits.BlendNext(ref result, ref value0, w0);
traits.BlendNext(ref result, ref value1, w1);
traits.BlendNext(ref result, ref value2, w2);
traits.EndBlend(ref result);
Assert.IsTrue(Vector3.AreNumericallyEqual(value0.Scale * w0 + value1.Scale * w1 + value2.Scale * w2, result.Scale));
Assert.IsTrue(Vector3.AreNumericallyEqual(value0.Translation * w0 + value1.Translation * w1 + value2.Translation * w2, result.Translation));
Quaternion expected;
expected = value0.Rotation * w0;
// Consider "selective negation" when blending quaternions!
if (Quaternion.Dot(expected, value1.Rotation) < 0)
{
value1.Rotation = -value1.Rotation;
}
expected += value1.Rotation * w1;
if (Quaternion.Dot(expected, value2.Rotation) < 0)
{
value2.Rotation = -value2.Rotation;
}
expected += value2.Rotation * w2;
expected.Normalize();
Assert.IsTrue(Quaternion.AreNumericallyEqual(expected, result.Rotation));
}
/// <summary>
/// Determines whether two SRT transforms are equal (within a numerical tolerance).
/// </summary>
/// <param name="srtA">The first transform.</param>
/// <param name="srtB">The second transform.</param>
/// <returns>
/// <see langword="true"/> if the given transforms are numerically equal; otherwise,
/// <see langword="false"/>.
/// </returns>
public static bool AreNumericallyEqual(SrtTransform srtA, SrtTransform srtB)
{
return(Quaternion.AreNumericallyEqual(srtA.Rotation, srtB.Rotation) &&
Vector3.AreNumericallyEqual(srtA.Translation, srtB.Translation) &&
Vector3.AreNumericallyEqual(srtA.Scale, srtB.Scale));
}
public void CreateRotation()
{
Quaternion q;
// From matrix vs. from angle/axis
Matrix m = Matrix.CreateRotation(Vector3.UnitX, (float)Math.PI / 4);
q = Quaternion.CreateFromRotationMatrix(m);
Quaternion q2 = Quaternion.CreateFromRotationMatrix(Vector3.UnitX, (float)Math.PI / 4);
Assert.IsTrue(Quaternion.AreNumericallyEqual(q2, q));
m = Matrix.CreateRotation(Vector3.UnitY, (float)Math.PI / 4);
q = Quaternion.CreateFromRotationMatrix(m);
q2 = Quaternion.CreateFromRotationMatrix(Vector3.UnitY, (float)Math.PI / 4);
Assert.IsTrue(Quaternion.AreNumericallyEqual(q2, q));
m = Matrix.CreateRotation(Vector3.UnitZ, (float)Math.PI / 4);
q = Quaternion.CreateFromRotationMatrix(m);
q2 = Quaternion.CreateFromRotationMatrix(Vector3.UnitZ, (float)Math.PI / 4);
Assert.IsTrue(Quaternion.AreNumericallyEqual(q2, q));
// From vector-vector
Vector3 start, end;
start = Vector3.UnitX;
end = Vector3.UnitY;
q = Quaternion.CreateFromRotationMatrix(start, end);
Assert.IsTrue(Vector3.AreNumericallyEqual(end, q.ToRotationMatrix33() * start));
start = Vector3.UnitY;
end = Vector3.UnitZ;
q = Quaternion.CreateFromRotationMatrix(start, end);
Assert.IsTrue(Vector3.AreNumericallyEqual(end, q.ToRotationMatrix33() * start));
start = Vector3.UnitZ;
end = Vector3.UnitX;
q = Quaternion.CreateFromRotationMatrix(start, end);
Assert.IsTrue(Vector3.AreNumericallyEqual(end, q.ToRotationMatrix33() * start));
start = new Vector3(1, 1, 1);
end = new Vector3(1, 1, 1);
q = Quaternion.CreateFromRotationMatrix(start, end);
Assert.IsTrue(Vector3.AreNumericallyEqual(end, q.ToRotationMatrix33() * start));
start = new Vector3(1.0f, 1.0f, 1.0f);
end = new Vector3(-1.0f, -1.0f, -1.0f);
q = Quaternion.CreateFromRotationMatrix(start, end);
Assert.IsTrue(Vector3.AreNumericallyEqual(end, q.ToRotationMatrix33() * start));
start = new Vector3(-1.0f, 2.0f, 1.0f);
end = new Vector3(-2.0f, -1.0f, -1.0f);
q = Quaternion.CreateFromRotationMatrix(start, end);
Assert.IsTrue(Vector3.AreNumericallyEqual(end, q.ToRotationMatrix33() * start));
float degree45 = MathHelper.ToRadians(45);
q = Quaternion.CreateFromRotationMatrix(degree45, Vector3.UnitZ, degree45, Vector3.UnitY, degree45, Vector3.UnitX, false);
Quaternion expected = Quaternion.CreateFromRotationMatrix(Vector3.UnitZ, degree45) * Quaternion.CreateFromRotationMatrix(Vector3.UnitY, degree45)
* Quaternion.CreateFromRotationMatrix(Vector3.UnitX, degree45);
Assert.IsTrue(Quaternion.AreNumericallyEqual(expected, q));
q = Quaternion.CreateFromRotationMatrix(degree45, Vector3.UnitZ, degree45, Vector3.UnitY, degree45, Vector3.UnitX, true);
expected = Quaternion.CreateFromRotationMatrix(Vector3.UnitX, degree45) * Quaternion.CreateFromRotationMatrix(Vector3.UnitY, degree45)
* Quaternion.CreateFromRotationMatrix(Vector3.UnitZ, degree45);
Assert.IsTrue(Quaternion.AreNumericallyEqual(expected, q));
}