public static Quaternion4D Squad(
Quaternion4D q1,
Quaternion4D a,
Quaternion4D b,
Quaternion4D c,
double t)
{
return(Slerp(
Slerp(q1, c, t), Slerp(a, b, t), 2 * t * (1.0 - t)));
}
public override bool Equals(object obj)
{
if (obj is Quaternion4D)
{
Quaternion4D q = (Quaternion4D)obj;
return(q == this);
}
else
{
return(false);
}
}
public static Vector3D QuaternionToEuler(Quaternion4D q)
{
double q0 = q.W;
double q1 = q.X;
double q2 = q.Y;
double q3 = q.Z;
double yaw = Math.Atan2(2 * (q2 * q3 + q0 * q1), (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3));
double pitch = Math.Asin(-2 * (q1 * q3 - q0 * q2));
double roll = Math.Atan2(2 * (q1 * q2 + q0 * q3), (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3));
return(new Vector3D(yaw, pitch, roll));
}
/// <summary>
/// Transforms a rotation in quaternion form to a set of Euler angles
/// </summary>
/// <returns>The rotation transformed to Euler angles, X=Yaw, Y=Pitch, Z=Roll (radians)</returns>
public static Vector3D QuaternionToEuler(Quaternion4D q)
{
double q0 = q.W;
double q1 = q.X;
double q2 = q.Y;
double q3 = q.Z;
double x = Math.Atan2(2 * (q2 * q3 + q0 * q1), (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3));
double y = Math.Asin(-2 * (q1 * q3 - q0 * q2));
double z = Math.Atan2(2 * (q1 * q2 + q0 * q3), (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3));
return(new Vector3D(x, y, z));
}
public static Quaternion4D Slerp(Quaternion4D q0, Quaternion4D q1, double t)
{
double cosom = q0.X * q1.X + q0.Y * q1.Y + q0.Z * q1.Z + q0.W * q1.W;
double tmp0, tmp1, tmp2, tmp3;
if (cosom < 0.0)
{
cosom = -cosom;
tmp0 = -q1.X;
tmp1 = -q1.Y;
tmp2 = -q1.Z;
tmp3 = -q1.W;
}
else
{
tmp0 = q1.X;
tmp1 = q1.Y;
tmp2 = q1.Z;
tmp3 = q1.W;
}
/* calc coeffs */
double scale0, scale1;
if ((1.0 - cosom) > double.Epsilon)
{
// standard case (slerp)
double omega = Math.Acos(cosom);
double sinom = Math.Sin(omega);
scale0 = Math.Sin((1.0 - t) * omega) / sinom;
scale1 = Math.Sin(t * omega) / sinom;
}
else
{
/* just lerp */
scale0 = 1.0 - t;
scale1 = t;
}
Quaternion4D q = new Quaternion4D();
q.X = scale0 * q0.X + scale1 * tmp0;
q.Y = scale0 * q0.Y + scale1 * tmp1;
q.Z = scale0 * q0.Z + scale1 * tmp2;
q.W = scale0 * q0.W + scale1 * tmp3;
return(q);
}
//the below functions have not been certified to work properly
public static Quaternion4D Exp(Quaternion4D q)
{
double sinom;
double om = Math.Sqrt(q.X * q.X + q.Y * q.Y + q.Z * q.Z);
if (Math.Abs(om) < double.Epsilon)
{
sinom = 1.0;
}
else
{
sinom = Math.Sin(om) / om;
}
q.X = q.X * sinom;
q.Y = q.Y * sinom;
q.Z = q.Z * sinom;
q.W = Math.Cos(om);
return(q);
}
public static Quaternion4D Ln(Quaternion4D q)
{
double t = 0;
double s = Math.Sqrt(q.X * q.X + q.Y * q.Y + q.Z * q.Z);
double om = Math.Atan2(s, q.W);
if (Math.Abs(s) < double.Epsilon)
{
t = 0.0f;
}
else
{
t = om / s;
}
q.X = q.X * t;
q.Y = q.Y * t;
q.Z = q.Z * t;
q.W = 0.0f;
return(q);
}
public static void SquadSetup(
ref Quaternion4D outA,
ref Quaternion4D outB,
ref Quaternion4D outC,
Quaternion4D q0,
Quaternion4D q1,
Quaternion4D q2,
Quaternion4D q3)
{
q0 = q0 + q1;
q0.Normalize();
q2 = q2 + q1;
q2.Normalize();
q3 = q3 + q1;
q3.Normalize();
q1.Normalize();
outA = q1 * Exp(-0.25 * (Ln(Exp(q1) * q2) + Ln(Exp(q1) * q0)));
outB = q2 * Exp(-0.25 * (Ln(Exp(q2) * q3) + Ln(Exp(q2) * q1)));
outC = q2;
}
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 double Abs(Quaternion4D q)
{
return(Math.Sqrt(Norm(q)));
}
public static double Norm(Quaternion4D q)
{
return(q.X * q.X + q.Y * q.Y + q.Z * q.Z + q.W * q.W);
}
