public Quaternion4d(Quaternion4d q)
{
this.x = q.x;
this.y = q.y;
this.z = q.z;
this.w = q.w;
}
public Quaternion4d Slerp(Quaternion4d @from, Quaternion4d to, double t)
{
if (t <= 0)
{
return(new Quaternion4d(@from));
}
else if (t >= 1)
{
return(new Quaternion4d(to));
}
else
{
var cosom = @from.x * to.x + @from.y * to.y + @from.z * to.z + @from.w * to.w;
var absCosom = Math.Abs(cosom);
double scale0;
double scale1;
if ((1 - absCosom) > 1e-6)
{
var omega = MathUtility.Safe_Acos(absCosom);
var sinom = 1.0 / Math.Sin(omega);
scale0 = Math.Sin((1.0 - t) * omega) * sinom;
scale1 = Math.Sin(t * omega) * sinom;
}
else
{
scale0 = 1 - t;
scale1 = t;
}
return(new Quaternion4d(scale0 * @from.x + scale1 * to.x,
scale0 * @from.y + scale1 * to.y,
scale0 * @from.z + scale1 * to.z,
scale0 * @from.w + scale1 * to.w).Normalized());
}
}
public static double Dot(Quaternion4d a, Quaternion4d b)
{
return(a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w);
}
public static bool operator ==(Quaternion4d lhs, Quaternion4d rhs)
{
return(Quaternion4d.Dot(lhs, rhs) > 0.999999f);
}