public void Axis_and_Angle_are_consistent_with_given_in_ctor()
{
var angle = 2.56;
var axis = new Vector3(4.1, -0.9, 1.3);
var rot = new Rotation3(angle, axis);
Expect(rot.Angle, Is.EqualTo(angle).Within(_tolerance));
Expect(Vector3.Distance(rot.Axis, axis.Normalized), Is.LessThan(_tolerance));
}
public static Vector2 ProductXY(Rotation3 r, Vector3 v)
{
var q1 = r.Quaternion * v;
var q2 = r.Quaternion.Conjugate;
var x = QuaternionUtils.ProductB(q1, q2);
var y = QuaternionUtils.ProductC(q1, q2);
return(new Vector2(x, y));
}
public void Rotation3_constructed_from_axis_and_angle_applies_according_to_Rodrigues_formula()
{
var angle = 2.56;
var axis = new Vector3(4.1, -0.9, 1.3).Normalized;
var rot = new Rotation3(angle, axis);
var vector = new Vector3(-1.23, 4.56, -7.89);
var rotatedVector = rot.Apply(vector);
var rodrigues = Math.Cos(angle) * vector + Math.Sin(angle) * axis.Cross(vector) + (1 - Math.Cos(angle)) * (axis * vector) * axis;
Expect(Vector3.Distance(rotatedVector, rodrigues), Is.LessThan(_tolerance));
}
public void Basis_quaternion_rotations_are_computed_from_their_axis_and_angle()
{
var one = new Rotation3(angle: 0, axis: new Vector3(0.1, 0.2, 0.3));
Expect(Quaternion.Distance(one.Quaternion, Quaternion.One), Is.LessThan(_tolerance));
var i = new Rotation3(angle: Math.PI, axis: Vector3.UnitX);
Expect(Quaternion.Distance(i.Quaternion, Quaternion.I), Is.LessThan(_tolerance));
var j = new Rotation3(angle: Math.PI, axis: Vector3.UnitY);
Expect(Quaternion.Distance(j.Quaternion, Quaternion.J), Is.LessThan(_tolerance));
var k = new Rotation3(angle: Math.PI, axis: Vector3.UnitZ);
Expect(Quaternion.Distance(k.Quaternion, Quaternion.K), Is.LessThan(_tolerance));
}
public void Axis_and_Angle_are_correct_for_basis_quaternions()
{
var one = new Rotation3(Quaternion.One);
Expect(one.Angle, Is.EqualTo(0).Within(_tolerance));
var i = new Rotation3(Quaternion.I);
ExpectAxisAngle(i, axis: Vector3.UnitX, angle: Math.PI);
var j = new Rotation3(Quaternion.J);
ExpectAxisAngle(j, axis: Vector3.UnitY, angle: Math.PI);
var k = new Rotation3(Quaternion.K);
ExpectAxisAngle(k, axis: Vector3.UnitZ, angle: Math.PI);
}
private void ExpectAxisAngle(Rotation3 rot, Vector3 axis, double angle)
{
Expect(rot.Angle, Is.EqualTo(angle).Within(_tolerance));
Expect(Vector3.Distance(rot.Axis, axis), Is.LessThan(_tolerance));
}
public void GramSchmidt_reconstructs_rotation_from_its_x_column_and_a_linear_combination_of_the_x_and_y_columns([ValueSource("GramSchmidt_TestCases")] Rotation3 rot)
{
var x = 4.3 * rot.ColX();
var y = 3.4 * rot.ColY() + 0.123 * x;
var result = Rotation3Utils.GramSchmidt(x, y);
ExpectEqualRotations(result, rot);
}
private void ExpectEqualRotations(Rotation3 rot1, Rotation3 rot2)
{
Expect(Quaternion.Distance(rot1, rot2) < _tolerance || Quaternion.Distance(rot1, -rot2.Quaternion) < _tolerance);
}
public static Vector3 RowZ(this Rotation3 @this)
{
return(QuaternionUtils.ProductIm(@this.Quaternion.Conjugate.TimesK(), @this.Quaternion));
}
public static double ProductZ(Rotation3 r, Vector3 v)
{
return(QuaternionUtils.ProductD(r.Quaternion * v, r.Quaternion.Conjugate));
}
public static Vector3 ColY(this Rotation3 @this)
{
return(QuaternionUtils.ProductIm(@this.Quaternion.TimesJ(), @this.Quaternion.Conjugate));
}
public static OSquareMatrix InvMatrix(this Rotation3 @this)
{
return(new SquareMatrix(@this.RowX(), @this.RowY(), @this.RowZ()));
}
public static OSquareMatrix Matrix(this Rotation3 @this)
{
return(new SquareMatrix(@this.ColX(), @this.ColY(), @this.ColZ()));
}
public YamlSerialization(Rotation3 r)
{
Axis = new Vector3Utils.YamlSerialization(r.Axis);
Angle = r.Angle;
}
public static double InvProductY(Rotation3 r, Vector3 v)
{
return(QuaternionUtils.ProductC(r.Quaternion.Conjugate * v, r.Quaternion));
}