public Quaternion(Quaternion quaterion)
{
tuple = new double[4];
tuple[0] = quaterion.tuple[0];
tuple[1] = quaterion.tuple[1];
tuple[2] = quaterion.tuple[2];
tuple[3] = quaterion.tuple[3];
}
public Quaternion Sub(Quaternion quaternion)
{
var result = new Quaternion();
for (int i = 0; i < 4; i++)
result.tuple[i] = tuple[i] - quaternion.tuple[i];
return result;
}
public Quaternion Negative()
{
var result = new Quaternion();
for (var i = 0; i < 4; i++)
result.tuple[i] = -tuple[i];
return result;
}
public Quaternion Mult(double scalar)
{
var result = new Quaternion();
for (var i = 0; i < 4; i++)
result.tuple[i] = tuple[i] * scalar;
return result;
}
public Quaternion Mult(Quaternion quaternion)
{
// NOTE: Multiplication is not generally commutative, so in most
// cases p*q != q*p.
return new Quaternion(
tuple[0] * quaternion.tuple[0] - tuple[1] * quaternion.tuple[1]
- tuple[2] * quaternion.tuple[2] - tuple[3] * quaternion.tuple[3],
tuple[0] * quaternion.tuple[1] + tuple[1] * quaternion.tuple[0]
+ tuple[2] * quaternion.tuple[3] - tuple[3] * quaternion.tuple[2],
tuple[0] * quaternion.tuple[2] + tuple[2] * quaternion.tuple[0]
+ tuple[3] * quaternion.tuple[1] - tuple[1] * quaternion.tuple[3],
tuple[0] * quaternion.tuple[3] + tuple[3] * quaternion.tuple[0]
+ tuple[1] * quaternion.tuple[2] - tuple[2] * quaternion.tuple[1]);
}
public Quaternion Log()
{
var result = new Quaternion();
// If q = cos(A)+sin(A)*(x*i+y*j+z*k) where (x,y,z) is unit length, then
// log(q) = A*(x*i+y*j+z*k). If sin(A) is near zero, use log(q) =
// sin(A)*(x*i+y*j+z*k) since sin(A)/A has limit 1.
result.tuple[0] = 0;
if (System.Math.Abs(tuple[0]) < 1)
{
var angle = System.Math.Acos(tuple[0]);
var sin = System.Math.Sin(angle);
if (System.Math.Abs(sin) > 0)
{
var coeff = angle / sin;
for (var i = 1; i <= 3; i++)
result.tuple[i] = coeff * tuple[i];
return result;
}
}
for (var i = 1; i <= 3; i++)
result.tuple[i] = tuple[i];
return result;
}
public Quaternion Inverse()
{
var result = new Quaternion();
double norm = 0;
for (var i = 0; i < 4; i++)
norm += tuple[i] * tuple[i];
if (norm > 0)
{
var invNorm = 1 / norm;
result.tuple[0] = +tuple[0] * invNorm;
result.tuple[1] = -tuple[1] * invNorm;
result.tuple[2] = -tuple[2] * invNorm;
result.tuple[3] = -tuple[3] * invNorm;
}
else
// return an invalid result to flag the error
for (var i = 0; i < 4; i++)
result.tuple[i] = 0;
return result;
}
public Quaternion Exp()
{
var result = new Quaternion();
// If q = A*(x*i+y*j+z*k) where (x,y,z) is unit length, then
// exp(q) = cos(A)+sin(A)*(x*i+y*j+z*k). If sin(A) is near zero,
// use exp(q) = cos(A)+A*(x*i+y*j+z*k) since A/sin(A) has limit 1.
var angle = System.Math.Sqrt(tuple[1] * tuple[1]
+ tuple[2] * tuple[2]
+ tuple[3] * tuple[3]);
var sin = System.Math.Sin(angle);
result.tuple[0] = System.Math.Cos(angle);
if (System.Math.Abs(sin) > 0)
{
var coeff = sin / angle;
for (var i = 1; i <= 3; i++)
result.tuple[i] = coeff * tuple[i];
}
else
for (var i = 1; i <= 3; i++)
result.tuple[i] = tuple[i];
return result;
}
public double Dot(Quaternion quaternion)
{
double dot = 0;
for (var i = 0; i < 4; i++)
dot += tuple[i] * quaternion.tuple[i];
return dot;
}
public Quaternion Add(Quaternion quaternion)
{
var result = new Quaternion();
for (var i = 0; i < 4; i++)
result.tuple[i] = tuple[i] + quaternion.tuple[i];
return result;
}
public static Quaternion Squad(double t, Quaternion q0, Quaternion a0, Quaternion a1, Quaternion q1)
{
var slerpT = 2 * t *(1 - t);
var slerpP = Slerp(t, q0, q1);
var slerpQ = Slerp(t, a0, a1);
return Slerp(slerpT, slerpP, slerpQ);
}
public static Quaternion SlerpExtraSpins(double t, Quaternion p, Quaternion q, int extraSpins)
{
var cos = p.Dot(q);
var angle = System.Math.Acos(cos);
if (System.Math.Abs(angle) > 0)
{
var sin = System.Math.Sin(angle);
var phase = System.Math.PI * extraSpins * t;
var invSin = 1 / sin;
var coeff0 = System.Math.Sin((1 - t) * angle - phase) * invSin;
var coeff1 = System.Math.Sin(t * angle + phase) * invSin;
return p.Mult(coeff0).Add(q.Mult(coeff1));
}
return p;
}
public static Quaternion Slerp(double t, Quaternion p, Quaternion q)
{
var cos = p.Dot(q);
var angle = System.Math.Acos(cos);
if (System.Math.Abs(angle) > 0)
{
var sin = System.Math.Sin(angle);
var invSin = 1 / sin;
var coeff0 = System.Math.Sin((1 - t) * angle) * invSin;
var coeff1 = System.Math.Sin(t * angle) * invSin;
return p.Mult(coeff0).Add(q.Mult(coeff1));
}
return p;
}
public static Quaternion Immediate(Quaternion q0, Quaternion q1, Quaternion q2)
{
// assert: Q0, Q1, Q2 all unit-length
var q1inv = q1.Conjugate();
var p0 = q1inv.Mult(q0);
var p2 = q1inv.Mult(q2);
var arg = (p0.Log().Add(p2.Log())).Mult(-0.25);
return q1.Mult(arg.Exp());
}
