public void Power3()
{
const double θ = 0.4;
Vector3D v = new Vector3D(2.3, 1.0, -2.0);
v.Normalize();
QuaternionD q = new QuaternionD(Math.Cos(θ), Math.Sin(θ) * v);
QuaternionD q2 = q;
q2.Power(2);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q * q, q2));
QuaternionD q3 = q;
q3.Power(3);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q * q * q, q3));
q2 = q;
q2.Power(-2);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q.Inverse * q.Inverse, q2));
q3 = q;
q3.Power(-3);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q.Inverse * q.Inverse * q.Inverse, q3));
}
public void DivisionScalar()
{
double s = 123.456;
QuaternionD q = new QuaternionD(1.0, 2.0, 3.0, 4.0);
QuaternionD expectedResult = new QuaternionD(1.0 / s, 2.0 / s, 3.0 / s, 4.0 / s);
QuaternionD result = QuaternionD.Divide(q, s);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(expectedResult, result));
}
public void FromMatrixWithZeroTrace()
{
QuaternionD q;
Matrix33D m = new Matrix33D(0, 1, 0,
0, 0, 1,
1, 0, 0);
q = QuaternionD.CreateRotation(m);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(new QuaternionD(-0.5, 0.5, 0.5, 0.5), q));
}
public void AreEqualWithEpsilon()
{
double epsilon = 0.001;
QuaternionD q1 = new QuaternionD(1.0, 2.0, 3.0, 4.0);
QuaternionD q2 = new QuaternionD(1.002, 2.002, 3.002, 4.002);
QuaternionD q3 = new QuaternionD(1.0001, 2.0001, 3.0001, 4.0001);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q1, q1, epsilon));
Assert.IsFalse(QuaternionD.AreNumericallyEqual(q1, q2, epsilon));
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q1, q3, epsilon));
}
public void Angle()
{
Vector3D axis = new Vector3D(1.0, 2.0, 3.0);
QuaternionD q = QuaternionD.CreateRotation(axis, 0.4);
Assert.IsTrue(Numeric.AreEqual(0.4, q.Angle));
q.Angle = 0.9;
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q, QuaternionD.CreateRotation(axis, 0.9)));
Assert.AreEqual(0, new QuaternionD(1.00000001f, 0, 0, 0).Angle);
}
public void Power()
{
const double θ = 0.4;
const double t = -1.2;
Vector3D v = new Vector3D(2.3, 1.0, -2.0);
v.Normalize();
QuaternionD q = new QuaternionD((double)Math.Cos(θ), (double)Math.Sin(θ) * v);
QuaternionD power = QuaternionD.Power(q, t);
QuaternionD expected = new QuaternionD((double)Math.Cos(t * θ), (double)Math.Sin(t * θ) * v);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(expected, power));
}
public void AreEqual()
{
double originalEpsilon = Numeric.EpsilonD;
Numeric.EpsilonD = 1e-8;
QuaternionD q1 = new QuaternionD(1.0, 2.0, 3.0, 4.0);
QuaternionD q2 = new QuaternionD(1.000001, 2.000001, 3.000001, 4.000001);
QuaternionD q3 = new QuaternionD(1.00000001, 2.00000001, 3.00000001, 4.00000001);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q1, q1));
Assert.IsFalse(QuaternionD.AreNumericallyEqual(q1, q2));
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q1, q3));
Numeric.EpsilonD = originalEpsilon;
}
public void Interpolate()
{
PoseD p1 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(0.3));
PoseD p2 = new PoseD(new Vector3D(-4, 5, -6), QuaternionD.CreateRotationZ(-0.1));
Assert.IsTrue(Vector3D.AreNumericallyEqual(p1.Position, PoseD.Interpolate(p1, p2, 0).Position));
Assert.IsTrue(Matrix33D.AreNumericallyEqual(p1.Orientation, PoseD.Interpolate(p1, p2, 0).Orientation));
Assert.IsTrue(Vector3D.AreNumericallyEqual(p2.Position, PoseD.Interpolate(p1, p2, 1).Position));
Assert.IsTrue(Matrix33D.AreNumericallyEqual(p2.Orientation, PoseD.Interpolate(p1, p2, 1).Orientation));
Assert.IsTrue(Vector3D.AreNumericallyEqual(InterpolationHelper.Lerp(p1.Position, p2.Position, 0.3), PoseD.Interpolate(p1, p2, 0.3).Position));
Assert.IsTrue(
QuaternionD.AreNumericallyEqual(
InterpolationHelper.Lerp(QuaternionD.CreateRotation(p1.Orientation), QuaternionD.CreateRotation(p2.Orientation), 0.3),
QuaternionD.CreateRotation(PoseD.Interpolate(p1, p2, 0.3).Orientation)));
}
public void SlerpDoublePrecision()
{
// Warning: The not all results are not verified
QuaternionD q1 = new QuaternionD(1.0, 2.0, 3.0, 4.0).Normalized;
QuaternionD q2 = new QuaternionD(2.0, 4.0, 6.0, 8.0).Normalized;
QuaternionD slerp = InterpolationHelper.Slerp(q1, q2, 0.75);
Assert.IsTrue(slerp.IsNumericallyNormalized);
slerp = InterpolationHelper.Slerp(q1, q2, 0);
Assert.IsTrue(slerp.IsNumericallyNormalized);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q1, slerp));
slerp = InterpolationHelper.Slerp(q1, q2, 1);
Assert.IsTrue(slerp.IsNumericallyNormalized);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q2, slerp));
}
public void LerpQuaternionD()
{
// Warning: The not all results are not verified
QuaternionD q1 = new QuaternionD(1.0, 2.0, 3.0, 4.0).Normalized;
QuaternionD q2 = new QuaternionD(2.0, 4.0, 6.0, 8.0).Normalized;
QuaternionD lerp = InterpolationHelper.Lerp(q1, q2, 0.75);
Assert.IsTrue(lerp.IsNumericallyNormalized);
lerp = InterpolationHelper.Lerp(q1, q2, 0);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q1, lerp));
lerp = InterpolationHelper.Lerp(q1, q2, 1);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q2, lerp));
q1 = QuaternionD.Identity;
q2 = QuaternionD.CreateRotation(Vector3D.UnitZ, Math.PI / 2);
lerp = InterpolationHelper.Lerp(q1, q2, 0.5);
Vector3D v = lerp.Rotate(Vector3D.UnitX);
Vector3D result = new Vector3D(1.0, 1.0, 0.0).Normalized;
Assert.IsTrue(Vector3D.AreNumericallyEqual(result, v));
q1 = QuaternionD.Identity;
q2 = QuaternionD.CreateRotation(Vector3D.UnitY, Math.PI / 2);
lerp = InterpolationHelper.Lerp(q1, q2, 0.5);
v = lerp.Rotate(Vector3D.UnitZ);
result = new Vector3D(1.0, 0.0, 1.0).Normalized;
Assert.IsTrue(Vector3D.AreNumericallyEqual(result, v));
q1 = QuaternionD.Identity;
q2 = QuaternionD.CreateRotation(Vector3D.UnitX, Math.PI / 2);
lerp = InterpolationHelper.Lerp(q1, q2, 0.5);
v = lerp.Rotate(Vector3D.UnitY);
result = new Vector3D(0.0, 1.0, 1.0).Normalized;
Assert.IsTrue(Vector3D.AreNumericallyEqual(result, v));
q1 = new QuaternionD(-1.0, 0.0, 0.0, 0.0);
q2 = QuaternionD.CreateRotation(-Vector3D.UnitZ, -Math.PI / 2);
lerp = InterpolationHelper.Lerp(q1, q2, 0.5);
v = lerp.Rotate(Vector3D.UnitX);
result = new Vector3D(1.0, 1.0, 0.0).Normalized;
Assert.IsTrue(Vector3D.AreNumericallyEqual(result, v));
}
public void Inverse()
{
QuaternionD identity = QuaternionD.Identity;
QuaternionD inverseIdentity = identity.Inverse;
Assert.AreEqual(inverseIdentity, identity);
double angle = 0.4;
Vector3D axis = new Vector3D(1.0, 1.0, 1.0);
axis.Normalize();
QuaternionD q = QuaternionD.CreateRotation(axis, angle);
QuaternionD inverse = q.Inverse;
Assert.IsTrue(Vector3D.AreNumericallyEqual(-axis, inverse.Axis));
q = new QuaternionD(1, 2, 3, 4);
inverse = q.Inverse;
Assert.IsTrue(QuaternionD.AreNumericallyEqual(QuaternionD.Identity, inverse * q));
}
public void SquadDoublePrecision()
{
QuaternionD q0 = QuaternionD.CreateRotation(new Vector3D(1, 1, 1), 0.3);
QuaternionD q1 = QuaternionD.CreateRotation(new Vector3D(1, 0, 1), 0.4);
QuaternionD q2 = QuaternionD.CreateRotation(new Vector3D(1, 0, -1), -0.6);
QuaternionD q3 = QuaternionD.CreateRotation(new Vector3D(0, 1, 1), 0.2);
QuaternionD q, a, b, p;
QuaternionD expected;
InterpolationHelper.SquadSetup(q0, q1, q2, q3, out q, out a, out b, out p);
// t = 0
QuaternionD result = InterpolationHelper.Squad(q, a, b, p, 0.0);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q1, result));
// t = 1.0f
result = InterpolationHelper.Squad(q, a, b, p, 1.0);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q2, result));
// Check series (just for debugging)
QuaternionD r1, r2, r3, r4, r5, r6, r7, r8, r9;
r1 = InterpolationHelper.Squad(q, a, b, p, 0.1);
r2 = InterpolationHelper.Squad(q, a, b, p, 0.2);
r3 = InterpolationHelper.Squad(q, a, b, p, 0.3);
r4 = InterpolationHelper.Squad(q, a, b, p, 0.4);
r5 = InterpolationHelper.Squad(q, a, b, p, 0.5);
r6 = InterpolationHelper.Squad(q, a, b, p, 0.6);
r7 = InterpolationHelper.Squad(q, a, b, p, 0.7);
r8 = InterpolationHelper.Squad(q, a, b, p, 0.8);
r9 = InterpolationHelper.Squad(q, a, b, p, 0.9);
// 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.5);
expected = InterpolationHelper.Slerp(q1, q2, 0.5);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(expected, result));
}
public void CreateRotation()
{
QuaternionD q;
// From matrix vs. from angle/axis
Matrix33D m = Matrix33D.CreateRotation(Vector3D.UnitX, (double)Math.PI / 4);
q = QuaternionD.CreateRotation(m);
QuaternionD q2 = QuaternionD.CreateRotation(Vector3D.UnitX, (double)Math.PI / 4);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q2, q));
m = Matrix33D.CreateRotation(Vector3D.UnitY, (double)Math.PI / 4);
q = QuaternionD.CreateRotation(m);
q2 = QuaternionD.CreateRotation(Vector3D.UnitY, (double)Math.PI / 4);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q2, q));
m = Matrix33D.CreateRotation(Vector3D.UnitZ, (double)Math.PI / 4);
q = QuaternionD.CreateRotation(m);
q2 = QuaternionD.CreateRotation(Vector3D.UnitZ, (double)Math.PI / 4);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q2, q));
// From vector-vector
Vector3D start, end;
start = Vector3D.UnitX;
end = Vector3D.UnitY;
q = QuaternionD.CreateRotation(start, end);
Assert.IsTrue(Vector3D.AreNumericallyEqual(end, q.ToRotationMatrix33() * start));
start = Vector3D.UnitY;
end = Vector3D.UnitZ;
q = QuaternionD.CreateRotation(start, end);
Assert.IsTrue(Vector3D.AreNumericallyEqual(end, q.ToRotationMatrix33() * start));
start = Vector3D.UnitZ;
end = Vector3D.UnitX;
q = QuaternionD.CreateRotation(start, end);
Assert.IsTrue(Vector3D.AreNumericallyEqual(end, q.ToRotationMatrix33() * start));
start = new Vector3D(1, 1, 1);
end = new Vector3D(1, 1, 1);
q = QuaternionD.CreateRotation(start, end);
Assert.IsTrue(Vector3D.AreNumericallyEqual(end, q.ToRotationMatrix33() * start));
start = new Vector3D(1.0, 1.0, 1.0);
end = new Vector3D(-1.0, -1.0, -1.0);
q = QuaternionD.CreateRotation(start, end);
Assert.IsTrue(Vector3D.AreNumericallyEqual(end, q.ToRotationMatrix33() * start));
start = new Vector3D(-1.0, 2.0, 1.0);
end = new Vector3D(-2.0, -1.0, -1.0);
q = QuaternionD.CreateRotation(start, end);
Assert.IsTrue(Vector3D.AreNumericallyEqual(end, q.ToRotationMatrix33() * start));
double degree45 = MathHelper.ToRadians(45);
q = QuaternionD.CreateRotation(degree45, Vector3D.UnitZ, degree45, Vector3D.UnitY, degree45, Vector3D.UnitX, false);
QuaternionD expected = QuaternionD.CreateRotation(Vector3D.UnitZ, degree45) * QuaternionD.CreateRotation(Vector3D.UnitY, degree45)
* QuaternionD.CreateRotation(Vector3D.UnitX, degree45);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(expected, q));
q = QuaternionD.CreateRotation(degree45, Vector3D.UnitZ, degree45, Vector3D.UnitY, degree45, Vector3D.UnitX, true);
expected = QuaternionD.CreateRotation(Vector3D.UnitX, degree45) * QuaternionD.CreateRotation(Vector3D.UnitY, degree45)
* QuaternionD.CreateRotation(Vector3D.UnitZ, degree45);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(expected, q));
}
