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