private static Quaternion SquadIntermediate(Quaternion previous, Quaternion current, Quaternion next) { Quaternion q1inv = Quaternion.Inverse(current); Quaternion c1 = q1inv * next; Quaternion c2 = q1inv * previous; c1.Log(); c2.Log(); Quaternion c3 = c2.Added(c1); c3.Scale(-0.25f); c3.Exp(); Quaternion r = current * c3; r.Normalize(); return(r); }
public void Add() { var q1 = new Quaternion(); q1.Set(1, 2, 3, 4); var q2 = q1.Added(new Quaternion(2, 3, 4, 5)); Assert.AreEqual(3, q2.X, 0.000001); Assert.AreEqual(5, q2.Y, 0.000001); Assert.AreEqual(7, q2.Z, 0.000001); Assert.AreEqual(9, q2.W, 0.000001); q1.Add(new Quaternion(2, 3, 4, 5)); Assert.AreEqual(3, q1.X, 0.000001); Assert.AreEqual(5, q1.Y, 0.000001); Assert.AreEqual(7, q1.Z, 0.000001); Assert.AreEqual(9, q1.W, 0.000001); }
public void ValueType_Quaternion() { Quaternion q1 = new Quaternion(new Vec(1, 0, 0), Math.PI); Assert.AreEqual("1,0,0,6.12323399573677E-17", q1.ToString()); q1 = new Quaternion(new Vec(1, 0, 0), new Vec(1, 2, 0)); Assert.AreEqual("0,0,0.525731112119134,0.85065080835204", q1.ToString()); var mat = new Mat(); mat.SetIdentity(); q1 = new Quaternion(mat); Assert.AreEqual("0,0,0,1", q1.ToString()); Assert.IsTrue(q1.IsEqual(new Quaternion(0, 0, 0, 1))); Assert.AreEqual(mat, q1.GetMatrix()); q1 = new Quaternion(new Vec(1, 0, 0), Math.PI); Vec vec = new Vec(); double angle = 0; q1.GetVectorAndAngle(ref vec, ref angle); Assert.AreEqual(Math.PI, angle); Assert.AreEqual(new Vec(1, 0, 0), vec); q1.SetEulerAngles(EulerSequence.EulerAngles, 1.0, 2.0, 3.0); double a = 0; double b = 0; double c = 0; q1.GetEulerAngles(EulerSequence.EulerAngles, ref a, ref b, ref c); Assert.AreEqual(1.0, a, 0.0000001); Assert.AreEqual(2.0, b, 0.0000001); Assert.AreEqual(3.0, c, 0.0000001); q1.SetIdent(); Assert.AreEqual("0,0,0,1", q1.ToString()); q1.Set(1, 2, 3, 4); Assert.AreEqual("1,2,3,4", q1.ToString()); q1.Set(1, 2, 3, 4); Assert.AreEqual("-1,-2,-3,4", q1.Reversed().ToString()); q1.Reverse(); Assert.AreEqual("-1,-2,-3,4", q1.ToString()); q1.Set(1, 2, 3, 4); Assert.AreEqual("-0.0333333333333333,-0.0666666666666667,-0.1,0.133333333333333", q1.Inverted().ToString()); q1.Invert(); Assert.AreEqual("-0.0333333333333333,-0.0666666666666667,-0.1,0.133333333333333", q1.ToString()); Assert.AreEqual(0.0333333333333333, q1.SquareNorm(), 0.000000000001); Assert.AreEqual(0.182574185835055, q1.Norm(), 0.000000000001); q1.Set(1, 2, 3, 4); Assert.AreEqual("2,4,6,8", q1.Scaled(2).ToString()); q1.Scale(2); Assert.AreEqual("2,4,6,8", q1.ToString()); q1.Set(2, 2, 0, 1); q1.StabilizeLength(); Assert.AreEqual("0.4,0.4,0,0.2", q1.ToString()); q1.Set(2, 2, 0, 1); Assert.AreEqual("0.666666666666667,0.666666666666667,0,0.333333333333333", q1.Normalized().ToString()); q1.Normalize(); Assert.AreEqual("0.666666666666667,0.666666666666667,0,0.333333333333333", q1.ToString()); q1.Set(2, 2, 0, 1); Assert.AreEqual("-2,-2,0,-1", q1.Negated().ToString()); q1.Set(1, 2, 3, 4); Assert.AreEqual("3,5,7,9", q1.Added(new Quaternion(2, 3, 4, 5)).ToString()); q1.Add(new Quaternion(2, 3, 4, 5)); Assert.AreEqual("3,5,7,9", q1.ToString()); q1.Set(1, 2, 3, 4); Assert.AreEqual("3,5,7,9", q1.Subtracted(new Quaternion(-2, -3, -4, -5)).ToString()); q1.Subtract(new Quaternion(-2, -3, -4, -5)); Assert.AreEqual("3,5,7,9", q1.ToString()); q1.Set(1, 2, 3, 4); Assert.AreEqual("12,24,30,0", q1.Multiplied(new Quaternion(2, 3, 4, 5)).ToString()); q1.Multiply(new Quaternion(2, 3, 4, 5)); Assert.AreEqual("12,24,30,0", q1.ToString()); q1.Set(1, 2, 3, 1); Assert.AreEqual(12, q1.Dot(new Quaternion(2, 2, 1, 3))); Assert.AreEqual(2.6192778317837444, q1.GetRotationAngle(), 0.00000000000001); q1 = new Quaternion(new Vec(1, 0, 0), Math.PI); Assert.AreEqual("1,-2,-3", q1.Multiply(new Vec(1, 2, 3)).ToString()); //gp_Quaternion gq1 = new gp_Quaternion(new gp_Vec(1, 0, 0), Math.PI); //TestContext.WriteLine(string.Format(CultureInfo.InvariantCulture, "{0},{1},{2},{3}", gq1.X(), gq1.Y(), gq1.Z(), gq1.W())); }