public static Vector4d AxisAngle(Quatd q)
{
double angle = 2 * Mathd.Acos(q.w);
double den = Mathd.Sqrt(1 - q.w * q.w);
if (den == 0)
{
return(new Vector4d(0, 0, -1, angle));
}
return(new Vector4d(q.x / den, q.y / den, q.z / den, angle));
}
public Quatd Slerp(Quatd b, double t)
{
// Calculate cosine
double cosom = x * b.x + y * b.y + z * b.z + w * b.w;
var to1 = new Quatd();
// Adjust signs if necessary
if (cosom < 0.0)
{
cosom = -cosom;
to1.x = -b.x;
to1.y = -b.y;
to1.z = -b.z;
to1.w = -b.w;
}
else
{
to1.x = b.x;
to1.y = b.y;
to1.z = b.z;
to1.w = b.w;
}
double sinom, scale0, scale1;
// Calculate coefficients
if (1.0 - cosom > Mathd.Epsilon)
{
// Standard case (Slerp)
double omega = Mathd.Acos(cosom);
sinom = Mathd.Sin(omega);
scale0 = Mathd.Sin((1.0f - t) * omega) / sinom;
scale1 = Mathd.Sin(t * omega) / sinom;
}
else
{
// Quatdernions are very close so we can do a linear interpolation
scale0 = 1.0f - t;
scale1 = t;
}
// Calculate final values
return(new Quatd
(
scale0 * x + scale1 * to1.x,
scale0 * y + scale1 * to1.y,
scale0 * z + scale1 * to1.z,
scale0 * w + scale1 * to1.w
));
}
public Transform2Dd InterpolateWith(Transform2Dd m, double c)
{
double r1 = Rotation;
double r2 = m.Rotation;
Vector2d s1 = Scale;
Vector2d s2 = m.Scale;
// Slerp rotation
var v1 = new Vector2d(Mathd.Cos(r1), Mathd.Sin(r1));
var v2 = new Vector2d(Mathd.Cos(r2), Mathd.Sin(r2));
double dot = v1.Dot(v2);
// Clamp dot to [-1, 1]
dot = dot < -1.0f ? -1.0f : (dot > 1.0f ? 1.0f : dot);
Vector2d v;
if (dot > 0.9995f)
{
// Linearly interpolate to avoid numerical precision issues
v = v1.LinearInterpolate(v2, c).Normalized();
}
else
{
double angle = c * Mathd.Acos(dot);
Vector2d v3 = (v2 - v1 * dot).Normalized();
v = v1 * Mathd.Cos(angle) + v3 * Mathd.Sin(angle);
}
// Extract parameters
Vector2d p1 = origin;
Vector2d p2 = m.origin;
// Construct matrix
var res = new Transform2Dd(Mathd.Atan2(v.y, v.x), p1.LinearInterpolate(p2, c));
Vector2d scale = s1.LinearInterpolate(s2, c);
res.x *= scale;
res.y *= scale;
return(res);
}
/// <summary>
/// Returns the result of the spherical linear interpolation between
/// this quaternion and `to` by amount `weight`, but without
/// checking if the rotation path is not bigger than 90 degrees.
/// </summary>
/// <param name="to">The destination quaternion for interpolation. Must be normalized.</param>
/// <param name="weight">A value on the range of 0.0 to 1.0, representing the amount of interpolation.</param>
/// <returns>The resulting quaternion of the interpolation.</returns>
public Quatd Slerpni(Quatd to, double weight)
{
double dot = Dot(to);
if (Mathd.Abs(dot) > 0.9999f)
{
return(this);
}
double theta = Mathd.Acos(dot);
double sinT = 1.0f / Mathd.Sin(theta);
double newFactor = Mathd.Sin(weight * theta) * sinT;
double invFactor = Mathd.Sin((1.0f - weight) * theta) * sinT;
return(new Quatd
(
invFactor * x + newFactor * to.x,
invFactor * y + newFactor * to.y,
invFactor * z + newFactor * to.z,
invFactor * w + newFactor * to.w
));
}
public Quatd Slerpni(Quatd b, double t)
{
double dot = Dot(b);
if (Mathd.Abs(dot) > 0.9999f)
{
return(this);
}
double theta = Mathd.Acos(dot);
double sinT = 1.0f / Mathd.Sin(theta);
double newFactor = Mathd.Sin(t * theta) * sinT;
double invFactor = Mathd.Sin((1.0f - t) * theta) * sinT;
return(new Quatd
(
invFactor * x + newFactor * b.x,
invFactor * y + newFactor * b.y,
invFactor * z + newFactor * b.z,
invFactor * w + newFactor * b.w
));
}
/// <summary>
/// Returns the result of the spherical linear interpolation between
/// this quaternion and `to` by amount `weight`.
///
/// Note: Both quaternions must be normalized.
/// </summary>
/// <param name="to">The destination quaternion for interpolation. Must be normalized.</param>
/// <param name="weight">A value on the range of 0.0 to 1.0, representing the amount of interpolation.</param>
/// <returns>The resulting quaternion of the interpolation.</returns>
public Quatd Slerp(Quatd to, double weight)
{
#if DEBUG
if (!IsNormalized())
{
throw new InvalidOperationException("Quatd is not normalized");
}
if (!to.IsNormalized())
{
throw new ArgumentException("Argument is not normalized", nameof(to));
}
#endif
// Calculate cosine.
double cosom = x * to.x + y * to.y + z * to.z + w * to.w;
var to1 = new Quatd();
// Adjust signs if necessary.
if (cosom < 0.0)
{
cosom = -cosom;
to1.x = -to.x;
to1.y = -to.y;
to1.z = -to.z;
to1.w = -to.w;
}
else
{
to1.x = to.x;
to1.y = to.y;
to1.z = to.z;
to1.w = to.w;
}
double sinom, scale0, scale1;
// Calculate coefficients.
if (1.0 - cosom > Mathd.Epsilon)
{
// Standard case (Slerp).
double omega = Mathd.Acos(cosom);
sinom = Mathd.Sin(omega);
scale0 = Mathd.Sin((1.0f - weight) * omega) / sinom;
scale1 = Mathd.Sin(weight * omega) / sinom;
}
else
{
// Quaternions are very close so we can do a linear interpolation.
scale0 = 1.0f - weight;
scale1 = weight;
}
// Calculate final values.
return(new Quatd
(
scale0 * x + scale1 * to1.x,
scale0 * y + scale1 * to1.y,
scale0 * z + scale1 * to1.z,
scale0 * w + scale1 * to1.w
));
}
