public Quaternion2 Subtract(Quaternion2 q)
{
if ((this._coord != q._coord))
{
q = q.ConvertTo(this._coord);
}
Quaternion2 m = new Quaternion2(this.Coord);
m.W = _w - q.W;
m.X = _x - q.X;
m.Y = _y - q.Y;
m.Z = _z - q.Z;
return(m);
}
public Quaternion2 Add(Quaternion2 q)
{
if ((this._coord != q._coord))
{
q = q.ConvertTo(this._coord);
}
Quaternion2 m = new Quaternion2(this.Coord);
m.W = _w + q.W;
m.X = _x + q.X;
m.Y = _y + q.Y;
m.Z = _z + q.Z;
return(m);
}
public Quaternion2 Mult(Quaternion2 q)
{
if ((this._coord != q._coord))
{
q = q.ConvertTo(this._coord);
}
Quaternion2 m = new Quaternion2(this.Coord);
m.W = W * q.W - X * q.X - Y * q.Y - Z * q.Z;
m.X = W * q.X + X * q.W + Y * q.Z - Z * q.Y;
m.Y = W * q.Y - X * q.Z + Y * q.W + Z * q.X;
m.Z = W * q.Z + X * q.Y - Y * q.X + Z * q.W;
return(m);
}
/// <summary>
/// Spherical linear interpolation of two rotations.
/// </summary>
/// <param name="q1">Initial rotation</param>
/// <param name="q2">Final rotation</param>
/// <param name="t">Interpolation parameter within range [0, 1]</param>
public static Quaternion2 SLERP(Quaternion2 q1, Quaternion2 q2, double t)
{
// Algorithm from https://en.wikipedia.org/wiki/Slerp
Quaternion2 qq1 = q1.Copy().Normalized;
Quaternion2 qq2 = q2.Copy().Normalized;
if (qq2.Coord != qq1.Coord)
{
qq2 = qq2.ConvertTo(qq1.Coord);
}
double dot = qq1.W * qq2.W + qq1.X * qq2.X + qq1.Y * qq2.Y + qq1.Z * qq2.Z;
const double threshold = 0.9995;
if (Abs(dot) > threshold)
{
// Using linear interpolation if two rotations are close
Quaternion2 res = qq1 + t * (qq2 - qq1);
res.Normalize();
return(res);
}
//Make sure to choose shortest path
if (dot < 0.0)
{
qq2 = -qq2;
dot = -dot;
}
double theta_0 = Acos(dot);
double theta = theta_0 * t;
qq2 = qq2 - qq1 * dot;
qq2.Normalize();
return(qq1 * Cos(theta) + qq2 * Sin(theta));
}
