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); }