public void InverseWithNearSingularMatrix()
{
Matrix33D m = new Matrix33D(0.0001, 0, 0,
0, 0.0001, 0,
0, 0, 0.0001);
Vector3D v = Vector3D.One;
Vector3D w = m * v;
Assert.IsTrue(Vector3D.AreNumericallyEqual(v, m.Inverse * w));
Assert.IsTrue(Matrix33D.AreNumericallyEqual(Matrix33D.Identity, m * m.Inverse));
}
public void HomogeneousDivide()
{
Vector4D v4 = new Vector4D(1.0, 2.0, 3.0, 1.0);
Vector3D v3 = Vector4D.HomogeneousDivide(v4);
Assert.AreEqual(new Vector3D(1.0, 2.0, 3.0), v3);
v4 = new Vector4D(1.0, 2.0, 3.0, 10.0);
v3 = Vector4D.HomogeneousDivide(v4);
Assert.IsTrue(Vector3D.AreNumericallyEqual(new Vector3D(1.0 / 10.0, 2.0 / 10.0, 3.0 / 10.0), v3));
}
public void CreateRotationZ()
{
double angle = (double)MathHelper.ToRadians(30);
Matrix33D m = Matrix33D.CreateRotationZ(angle);
Assert.IsTrue(Vector3D.AreNumericallyEqual(new Vector3D((double)Math.Cos(angle), (double)Math.Sin(angle), 0), m * Vector3D.UnitX));
QuaternionD q = QuaternionD.CreateRotation(Vector3D.UnitZ, angle);
Assert.IsTrue(Vector3D.AreNumericallyEqual(q.Rotate(Vector3D.One), m * Vector3D.One));
Assert.IsTrue(Matrix33D.AreNumericallyEqual(Matrix33D.CreateRotation(Vector3D.UnitZ, angle), m));
}
public void AreEqualWithEpsilon()
{
double epsilon = 0.001;
Vector3D u = new Vector3D(1.0, 2.0, 3.0);
Vector3D v = new Vector3D(1.002, 2.002, 3.002);
Vector3D w = new Vector3D(1.0001, 2.0001, 3.0001);
Assert.IsTrue(Vector3D.AreNumericallyEqual(u, u, epsilon));
Assert.IsFalse(Vector3D.AreNumericallyEqual(u, v, epsilon));
Assert.IsTrue(Vector3D.AreNumericallyEqual(u, w, epsilon));
}
public void Inverse()
{
Assert.AreEqual(Matrix33D.Identity, Matrix33D.Identity.Inverse);
Matrix33D m = new Matrix33D(1, 2, 3,
2, 5, 8,
7, 6, -1);
Vector3D v = Vector3D.One;
Vector3D w = m * v;
Assert.IsTrue(Vector3D.AreNumericallyEqual(v, m.Inverse * w));
Assert.IsTrue(Matrix33D.AreNumericallyEqual(Matrix33D.Identity, m * m.Inverse));
}
public void Ln4()
{
double θ = 0.0;
Vector3D v = new Vector3D(1.0, 2.0, 3.0);
v.Normalize();
QuaternionD q = new QuaternionD((double)Math.Cos(θ), (double)Math.Sin(θ) * v);
q.Ln();
Assert.IsTrue(Numeric.AreEqual(0.0, q.W));
Assert.IsTrue(Vector3D.AreNumericallyEqual(θ * v, q.V));
}
public void SlerpNegatedDoublePrecision()
{
QuaternionD q1 = new QuaternionD(-1.0, 0.0, 0.0, 0.0);
QuaternionD q2 = QuaternionD.CreateRotation(-Vector3.UnitZ, -Math.PI / 2);
QuaternionD slerp = InterpolationHelper.Slerp(q1, q2, 0.5);
Assert.IsTrue(slerp.IsNumericallyNormalized);
Vector3D v = slerp.Rotate(Vector3D.UnitX);
Vector3D result = new Vector3D(1.0, 1.0, 0.0).Normalized;
Assert.IsTrue(Vector3D.AreNumericallyEqual(result, v));
}
public void SlerpXDoublePrecision()
{
QuaternionD q1 = QuaternionD.Identity;
QuaternionD q2 = QuaternionD.CreateRotation(Vector3.UnitX, Math.PI / 2);
QuaternionD slerp = InterpolationHelper.Slerp(q1, q2, 0.5);
Assert.IsTrue(slerp.IsNumericallyNormalized);
Vector3D v = slerp.Rotate(Vector3.UnitY);
Vector3D result = new Vector3D(0.0, 1.0, 1.0).Normalized;
Assert.IsTrue(Vector3D.AreNumericallyEqual(result, v));
}
public void CosineInterpolationVector3D()
{
Vector3D v = new Vector3D(1.0, 10.0, 100.0);
Vector3D w = new Vector3D(2.0, 20.0, 200.0);
Vector3D lerp0 = InterpolationHelper.CosineInterpolation(v, w, 0.0);
Vector3D lerp1 = InterpolationHelper.CosineInterpolation(v, w, 1.0);
Vector3D lerp05 = InterpolationHelper.CosineInterpolation(v, w, 0.5);
Assert.AreEqual(v, lerp0);
Assert.AreEqual(w, lerp1);
Assert.IsTrue(Vector3D.AreNumericallyEqual(new Vector3D(1.5, 15.0, 150.0), lerp05));
}
public void RotationMatrix33()
{
double angle = -1.6;
Vector3D axis = new Vector3D(1.0, 2.0, -3.0);
QuaternionD q = QuaternionD.CreateRotation(axis, angle);
Matrix33D m33 = Matrix33D.CreateRotation(axis, angle);
Vector3D v = new Vector3D(0.3, -2.4, 5.6);
Vector3D result1 = q.ToRotationMatrix33() * v;
Vector3D result2 = m33 * v;
Assert.IsTrue(Vector3D.AreNumericallyEqual(result1, result2));
}
public void Exp4()
{
double θ = 0.0;
Vector3D v = new Vector3D(1.0, 2.0, 3.0);
v.Normalize();
QuaternionD q = new QuaternionD(0.0, θ * v);
q.Exp();
Assert.IsTrue(Numeric.AreEqual(1, q.W));
Assert.IsTrue(Vector3D.AreNumericallyEqual(Vector3D.Zero, q.V));
}
public void Exp2()
{
double θ = -0.3;
Vector3D v = new Vector3D(1.0, 2.0, 3.0);
v.Normalize();
QuaternionD q = new QuaternionD(0.0, θ * v);
q.Exp();
Assert.IsTrue(Numeric.AreEqual((double)Math.Cos(θ), q.W));
Assert.IsTrue(Vector3D.AreNumericallyEqual((double)Math.Sin(θ) * v, q.V));
}
public void CreateRotation()
{
Matrix33D m = Matrix33D.CreateRotation(Vector3D.UnitX, 0.0);
Assert.AreEqual(Matrix33D.Identity, m);
m = Matrix33D.CreateRotation(Vector3D.UnitX, (double)Math.PI / 2);
Assert.IsTrue(Vector3D.AreNumericallyEqual(Vector3D.UnitZ, m * Vector3D.UnitY));
m = Matrix33D.CreateRotation(Vector3D.UnitY, (double)Math.PI / 2);
Assert.IsTrue(Vector3D.AreNumericallyEqual(Vector3D.UnitX, m * Vector3D.UnitZ));
m = Matrix33D.CreateRotation(Vector3D.UnitZ, (double)Math.PI / 2);
Assert.IsTrue(Vector3D.AreNumericallyEqual(Vector3D.UnitY, m * Vector3D.UnitX));
}
public void MultiplyVector()
{
Vector3D v = new Vector3D(2.34, 3.45, 4.56);
Assert.AreEqual(v, Matrix33D.Multiply(Matrix33D.Identity, v));
Assert.AreEqual(Vector3D.Zero, Matrix33D.Multiply(Matrix33D.Zero, v));
Matrix33D m = new Matrix33D(12, 23, 45,
67, 89, 90,
43, 65, 87);
Assert.IsTrue(Vector3D.AreNumericallyEqual(v, Matrix33D.Multiply(m * m.Inverse, v)));
for (int i = 0; i < 3; i++)
Assert.AreEqual(Vector3D.Dot(m.GetRow(i), v), Matrix33D.Multiply(m, v)[i]);
}
public void QuaternionDFromMatrix33()
{
Vector3D v = Vector3D.One;
Matrix33D m = Matrix33D.Identity;
QuaternionD q = QuaternionD.CreateRotation(m);
Assert.IsTrue(Vector3D.AreNumericallyEqual(m * v, q.Rotate(v)));
m = Matrix33D.CreateRotation(Vector3D.UnitX, 0.3);
q = QuaternionD.CreateRotation(m);
Assert.IsTrue(Vector3D.AreNumericallyEqual(m * v, q.Rotate(v)));
m = Matrix33D.CreateRotation(Vector3D.UnitY, 1.0);
q = QuaternionD.CreateRotation(m);
Assert.IsTrue(Vector3D.AreNumericallyEqual(m * v, q.Rotate(v)));
m = Matrix33D.CreateRotation(Vector3D.UnitZ, 4.0);
q = QuaternionD.CreateRotation(m);
Assert.IsTrue(Vector3D.AreNumericallyEqual(m * v, q.Rotate(v)));
m = Matrix33D.Identity;
q = QuaternionD.CreateRotation(m);
Assert.IsTrue(Vector3D.AreNumericallyEqual(m * v, q.Rotate(v)));
m = Matrix33D.CreateRotation(-Vector3D.UnitX, 1.3);
q = QuaternionD.CreateRotation(m);
Assert.IsTrue(Vector3D.AreNumericallyEqual(m * v, q.Rotate(v)));
m = Matrix33D.CreateRotation(-Vector3D.UnitY, -1.4);
q = QuaternionD.CreateRotation(m);
Assert.IsTrue(Vector3D.AreNumericallyEqual(m * v, q.Rotate(v)));
m = Matrix33D.CreateRotation(-Vector3D.UnitZ, -0.1);
q = QuaternionD.CreateRotation(m);
Assert.IsTrue(Vector3D.AreNumericallyEqual(m * v, q.Rotate(v)));
m = new Matrix33D(0, 0, 1,
0, -1, 0,
1, 0, 0);
q = QuaternionD.CreateRotation(m);
Assert.IsTrue(Vector3D.AreNumericallyEqual(m * v, q.Rotate(v)));
m = new Matrix33D(-1, 0, 0,
0, 1, 0,
0, 0, -1);
q = QuaternionD.CreateRotation(m);
Assert.IsTrue(Vector3D.AreNumericallyEqual(m * v, q.Rotate(v)));
}
public void AreEqual()
{
double originalEpsilon = Numeric.EpsilonD;
Numeric.EpsilonD = 1e-8;
Vector3D u = new Vector3D(1.0, 2.0, 3.0);
Vector3D v = new Vector3D(1.000001, 2.000001, 3.000001);
Vector3D w = new Vector3D(1.00000001, 2.00000001, 3.00000001);
Assert.IsTrue(Vector3D.AreNumericallyEqual(u, u));
Assert.IsFalse(Vector3D.AreNumericallyEqual(u, v));
Assert.IsTrue(Vector3D.AreNumericallyEqual(u, w));
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 Division()
{
double angle1 = 0.4;
Vector3D axis1 = new Vector3D(1.0, 2.0, 3.0);
QuaternionD q1 = QuaternionD.CreateRotation(axis1, angle1);
Matrix33D m1 = Matrix33D.CreateRotation(axis1, angle1);
double angle2 = -1.6;
Vector3D axis2 = new Vector3D(1.0, -2.0, -3.5);
QuaternionD q2 = QuaternionD.CreateRotation(axis2, angle2);
Matrix33D m2 = Matrix33D.CreateRotation(axis2, angle2);
Vector3D v = new Vector3D(0.3, -2.4, 5.6);
Vector3D result1 = QuaternionD.Divide(q2, q1).Rotate(v);
Vector3D result2 = m2 * m1.Inverse * v;
Assert.IsTrue(Vector3D.AreNumericallyEqual(result1, result2));
}
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 Test1()
{
PoseD p = PoseD.Identity;
Assert.AreEqual(Matrix44D.Identity, p.ToMatrix44D());
Assert.AreEqual(Matrix33D.Identity, p.Orientation);
Assert.AreEqual(Vector3D.Zero, p.Position);
p.Position = new Vector3D(1, 2, 3);
p.Orientation = Matrix33D.CreateRotation(new Vector3D(3, -4, 9), 0.49);
Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToWorldDirection(Vector3D.UnitX), 0), p * new Vector4D(1, 0, 0, 0)));
Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToWorldDirection(Vector3D.UnitY), 0), p * new Vector4D(0, 1, 0, 0)));
Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToWorldDirection(Vector3D.UnitZ), 0), p * new Vector4D(0, 0, 1, 0)));
Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToWorldPosition(Vector3D.UnitX), 1), p * new Vector4D(1, 0, 0, 1)));
Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToWorldPosition(Vector3D.UnitY), 1), p * new Vector4D(0, 1, 0, 1)));
Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToWorldPosition(Vector3D.UnitZ), 1), p * new Vector4D(0, 0, 1, 1)));
Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToLocalDirection(Vector3D.UnitX), 0), p.Inverse * new Vector4D(1, 0, 0, 0)));
Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToLocalDirection(Vector3D.UnitY), 0), p.Inverse * new Vector4D(0, 1, 0, 0)));
Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToLocalDirection(Vector3D.UnitZ), 0), p.Inverse * new Vector4D(0, 0, 1, 0)));
Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToLocalPosition(Vector3D.UnitX), 1), p.Inverse * new Vector4D(1, 0, 0, 1)));
Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToLocalPosition(Vector3D.UnitY), 1), p.Inverse * new Vector4D(0, 1, 0, 1)));
Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToLocalPosition(Vector3D.UnitZ), 1), p.Inverse * new Vector4D(0, 0, 1, 1)));
PoseD p2 = PoseD.FromMatrix(new Matrix44D(p.Orientation, Vector3D.Zero));
Assert.IsTrue(Matrix33D.AreNumericallyEqual(p.Orientation, p2.Orientation));
Assert.IsTrue(Vector3D.AreNumericallyEqual(p2.Position, Vector3D.Zero));
Matrix44D m = p2;
m.SetColumn(3, new Vector4D(p.Position, 1));
p2 = PoseD.FromMatrix(m);
Assert.IsTrue(Matrix33D.AreNumericallyEqual(p.Orientation, p2.Orientation));
Assert.AreEqual(p.Position, p2.Position);
//Assert.IsTrue(Vector3D.AreNumericallyEqual(p.Position, p2.Position));
// Test other constructors.
Assert.AreEqual(Vector3D.Zero, new PoseD(QuaternionD.CreateRotationX(0.3)).Position);
Assert.AreEqual(Matrix33D.CreateRotationX(0.3), new PoseD(Matrix33D.CreateRotationX(0.3)).Orientation);
Assert.AreEqual(new Vector3D(1, 2, 3), new PoseD(new Vector3D(1, 2, 3)).Position);
Assert.AreEqual(Matrix33D.Identity, new PoseD(new Vector3D(1, 2, 3)).Orientation);
}
public void Axis()
{
Vector3D axis = new Vector3D(1.0, 2.0, 3.0);
double angle = 0.2;
QuaternionD q = QuaternionD.CreateRotation(axis, angle);
Assert.IsTrue(Numeric.AreEqual(angle, q.Angle));
Assert.IsTrue(Vector3D.AreNumericallyEqual(axis.Normalized, q.Axis));
axis = new Vector3D(1.0, 1.0, 1.0);
q.Axis = axis;
Assert.IsTrue(Numeric.AreEqual(angle, q.Angle));
Assert.IsTrue(Vector3D.AreNumericallyEqual(axis.Normalized, q.Axis));
Assert.IsTrue(Vector3D.AreNumericallyEqual(Matrix33D.CreateRotation(axis, angle) * Vector3D.One, q.Rotate(Vector3D.One)));
Assert.AreEqual(Vector3D.Zero, QuaternionD.Identity.Axis);
q.Axis = Vector3D.Zero;
Assert.AreEqual(QuaternionD.Identity, q);
}
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 SlerpGeneralDoublePrecision()
{
QuaternionD q1 = QuaternionD.CreateRotation(-Vector3D.UnitY, Math.PI / 2);
QuaternionD q2 = QuaternionD.CreateRotation(Vector3D.UnitZ, Math.PI / 2);
QuaternionD slerp = InterpolationHelper.Slerp(q1, q2, 0.5);
Assert.IsTrue(slerp.IsNumericallyNormalized);
Vector3D v = slerp.Rotate(Vector3D.UnitX);
Vector3D result = new Vector3D(1.0 / 3.0, 2.0 / 3.0, 2.0 / 3.0); // I hope this is correct.
Assert.IsTrue(Vector3D.AreNumericallyEqual(result, v));
q1 = QuaternionD.CreateRotation(-Vector3D.UnitY, Math.PI / 2);
q2 = QuaternionD.CreateRotation(-Vector3D.UnitZ, -Math.PI / 2);
slerp = InterpolationHelper.Slerp(q1, q2, 0.5);
Assert.IsTrue(slerp.IsNumericallyNormalized);
v = slerp.Rotate(Vector3D.UnitX);
result = new Vector3D(1.0 / 3.0, 2.0 / 3.0, 2.0 / 3.0); // I hope this is correct.
Assert.IsTrue(Vector3D.AreNumericallyEqual(result, v));
}
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));
}