public void A_hashcode_should_be_returned()
{
const int expected = -2119088742;
var testQuaternion = new Quaternion(42, 66, 23);
var actual = testQuaternion.GetHashCode();
Assert.AreEqual(actual, expected);
}
public void A_quaternion_with_Euler_angle_theta_as_initial_parameter_should_be_created()
{
var testQuaternion = new Quaternion(EulerAngles.eTht, DegToRad(42.0));
Assert.AreEqual(testQuaternion.Q1, 0.93358042649, 1E-10);
Assert.AreEqual(testQuaternion.Q2, 0.0, 1E-10);
Assert.AreEqual(testQuaternion.Q3, 0.35836794954, 1E-10);
Assert.AreEqual(testQuaternion.Q4, 0.0, 1E-10);
}
public void A_quaternion_should_be_multiplied_with_a_quaternion_correctly()
{
var firstQuaternion = new Quaternion(1, 2, 3, 4);
var secondQuaternion = new Quaternion(-3, 5, 1, 8);
var actual = firstQuaternion * secondQuaternion;
Assert.AreEqual(actual[1], -48, 1E-08);
Assert.AreEqual(actual[2], 19, 1E-08);
Assert.AreEqual(actual[3], -4, 1E-08);
Assert.AreEqual(actual[4], -17, 1E-08);
}
public void A_quaternion_should_be_divided_by_a_scalar_correctly()
{
var testQuaternion = new Quaternion(1.5, 3, 4.5, 6);
const double scalar = 1.5;
var actual = testQuaternion / scalar;
Assert.AreEqual(actual[1], 1, 1E-08);
Assert.AreEqual(actual[2], 2, 1E-08);
Assert.AreEqual(actual[3], 3, 1E-08);
Assert.AreEqual(actual[4], 4, 1E-08);
}
public void A_quaternion_should_be_multiplied_with_a_scalar_correctly()
{
var testQuaternion = new Quaternion(1, 2, 3, 4);
const double scalar = 1.5;
var actual = testQuaternion * scalar;
Assert.AreEqual(actual[1], 1.5);
Assert.AreEqual(actual[2], 3);
Assert.AreEqual(actual[3], 4.5);
Assert.AreEqual(actual[4], 6);
}
public void Two_quaternions_should_be_unequal()
{
var firstQuaternion = new Quaternion(1, 2, 3, 4);
var secondQuaternion = new Quaternion(1, 3, 2, 4);
var actual = firstQuaternion != secondQuaternion;
Assert.AreEqual(actual, true);
}
public void A_user_defined_quaternion_should_be_created()
{
var testQuaternion = new Quaternion(42.0, -66.0, 23.5, -87.2);
Assert.AreEqual(testQuaternion.Q1, 42.0, 1E-08);
Assert.AreEqual(testQuaternion.Q2, -66.0, 1E-08);
Assert.AreEqual(testQuaternion.Q3, 23.5, 1E-08);
Assert.AreEqual(testQuaternion.Q4, -87.2, 1E-08);
}
/** Quaternion derivative for given angular rates.
Computes the quaternion derivative which results from the given
angular velocities
@param PQR a constant reference to the body rate vector
@return the quaternion derivative
@see Stevens and Lewis, "Aircraft Control and Simulation", Second Edition,
Equation 1.3-36. */
public Quaternion GetQDot(Vector3 pqr)
{
var norm = Magnitude();
if (Math.Abs(norm) < EqualityTolerance)
{
return Zero();
}
var rnorm = 1.0/norm;
var qDot = new Quaternion();
qDot[1] = -0.5*(Q2*pqr[(int) Rates.eP] + Q3*pqr[(int) Rates.eQ] + Q4*pqr[(int) Rates.eR]);
qDot[2] = 0.5*(Q1*pqr[(int) Rates.eP] + Q3*pqr[(int) Rates.eR] - Q4*pqr[(int) Rates.eQ]);
qDot[3] = 0.5*(Q1*pqr[(int) Rates.eQ] + Q4*pqr[(int) Rates.eP] - Q2*pqr[(int) Rates.eR]);
qDot[4] = 0.5*(Q1*pqr[(int) Rates.eR] + Q2*pqr[(int) Rates.eQ] - Q3*pqr[(int) Rates.eP]);
return rnorm*qDot;
}
public static Quaternion operator -(Quaternion first, Quaternion second)
{
var result = new Quaternion
(
first[1] - second[1],
first[2] - second[2],
first[3] - second[3],
first[4] - second[4]
);
return result;
}
public void A_quaternion_with_three_Euler_angles_as_initial_parameters_should_be_created()
{
var testQuaternion = new Quaternion(DegToRad(42.0), DegToRad(23.0), DegToRad(66.0));
Assert.AreEqual(testQuaternion.Q1, 0.80616101177, 1E-10);
Assert.AreEqual(testQuaternion.Q2, 0.19314748280, 1E-10);
Assert.AreEqual(testQuaternion.Q3, 0.34736125366, 1E-10);
Assert.AreEqual(testQuaternion.Q4, 0.43833620936, 1E-10);
}
public void Invert_a_quaternion()
{
var testQuaternion = new Quaternion(-4, 8, 1, 9);
var actual = testQuaternion.Inverse();
Assert.AreEqual(actual.Q1, -0.31426968052, 1E-10);
Assert.AreEqual(actual.Q2, -0.62853936105, 1E-10);
Assert.AreEqual(actual.Q3, -0.07856742013, 1E-10);
Assert.AreEqual(actual.Q4, -0.70710678118, 1E-10);
}
public void Conjugate_a_quaternion()
{
var testQuaternion = new Quaternion(-4, 8, 1, 9);
var actual = testQuaternion.Conjugate();
Assert.AreEqual(actual.Q1, -4, 1E-10);
Assert.AreEqual(actual.Q2, -8, 1E-10);
Assert.AreEqual(actual.Q3, -1, 1E-10);
Assert.AreEqual(actual.Q4, -9, 1E-10);
}
public void Calculate_square_magnitude_of_a_quaternion()
{
var testQuaternion = new Quaternion(-4, 8, 1, 9);
var actual = testQuaternion.SqrMagnitude();
Assert.AreEqual(actual, 162, 1E-08);
}
public void Calculate_Qdot()
{
var expectedQ1 = -23 / Math.Sqrt(46);
var expectedQ2 = -15 / Math.Sqrt(46);
var expectedQ3 = 4 / Math.Sqrt(46);
var expectedQ4 = 9 / Math.Sqrt(46);
var testVector = new Vector3(-1, 8, 3);
var testQuaternion = new Quaternion(2, 1, 4, 5);
var actual = testQuaternion.GetQDot(testVector);
Assert.AreEqual(actual.Q1, expectedQ1, 1E-10);
Assert.AreEqual(actual.Q2, expectedQ2, 1E-10);
Assert.AreEqual(actual.Q3, expectedQ3, 1E-10);
Assert.AreEqual(actual.Q4, expectedQ4, 1E-10);
}
public void Calculate_magnitude_of_a_quaternion()
{
var testQuaternion = new Quaternion(-4, 8, 1, 9);
var actual = testQuaternion.Magnitude();
Assert.AreEqual(actual, 12.72792206135, 1E-10);
}
public void Normalize_a_quaternion()
{
var testQuaternion = new Quaternion(-4, 8, 1, 9);
testQuaternion.Normalize();
Assert.AreEqual(testQuaternion.Q1, -0.31426968052, 1E-10);
Assert.AreEqual(testQuaternion.Q2, 0.62853936105, 1E-10);
Assert.AreEqual(testQuaternion.Q3, 0.07856742013, 1E-10);
Assert.AreEqual(testQuaternion.Q4, 0.70710678118, 1E-10);
}
public void Two_quaternions_should_be_added_correctly()
{
var firstQuaternion = new Quaternion(1, 2, 3, 4);
var secondQuaternion = new Quaternion(5, -2, 9, 3);
var actual = firstQuaternion + secondQuaternion;
Assert.AreEqual(actual[1], 6);
Assert.AreEqual(actual[2], 0);
Assert.AreEqual(actual[3], 12);
Assert.AreEqual(actual[4], 7);
}
public static Quaternion operator *(Quaternion first, Quaternion second)
{
var result = new Quaternion
(
first.Q1*second.Q1 - first.Q2*second.Q2 - first.Q3*second.Q3 - first.Q4*second.Q4,
first.Q1*second.Q2 + first.Q2*second.Q1 + first.Q3*second.Q4 - first.Q4*second.Q3,
first.Q1*second.Q3 - first.Q2*second.Q4 + first.Q3*second.Q1 + first.Q4*second.Q2,
first.Q1*second.Q4 + first.Q2*second.Q3 - first.Q3*second.Q2 + first.Q4*second.Q1
);
return result;
}
public void Two_quaternions_should_be_equal_using_equals()
{
var firstQuaternion = new Quaternion(1, 2, 3, 4);
var secondQuaternion = new Quaternion(1, 2, 3, 4);
var actual = firstQuaternion.Equals(secondQuaternion);
Assert.AreEqual(actual, true);
}
public static Quaternion operator /(Quaternion first, double scalar)
{
var result = new Quaternion
(
first[1]/scalar,
first[2]/scalar,
first[3]/scalar,
first[4]/scalar
);
return result;
}
public void Two_quaternions_should_be_subtracted_correctly()
{
var firstQuaternion = new Quaternion(1, 2, 3, 4);
var secondQuaternion = new Quaternion(5, -2, 9, 3);
var actual = firstQuaternion - secondQuaternion;
Assert.AreEqual(actual[1], -4);
Assert.AreEqual(actual[2], 4);
Assert.AreEqual(actual[3], -6);
Assert.AreEqual(actual[4], 1);
}
protected bool Equals(Quaternion other)
{
return Q1.Equals(other.Q1) && Q2.Equals(other.Q2) && Q3.Equals(other.Q3) && Q4.Equals(other.Q4);
}
public void A_standard_quaternion_should_be_created()
{
var testQuaternion = new Quaternion();
Assert.AreEqual(testQuaternion.Q1, 1.0, 1E-08);
Assert.AreEqual(testQuaternion.Q2, 0.0, 1E-08);
Assert.AreEqual(testQuaternion.Q3, 0.0, 1E-08);
Assert.AreEqual(testQuaternion.Q4, 0.0, 1E-08);
}
