/// <summary>
/// Sets this quaternion to result of normalized linear interpolation using a
/// different algorithm.
/// </summary>
/// <param name="start"> Starting quaternion in interpolation.</param>
/// <param name="end"> Ending quaternion in interpolation.</param>
/// <param name="amount">
/// Number between 0 and 1 that sets position of resulting quaternion between
/// <paramref name="start"/> and <paramref name="end"/>.
/// </param>
public void NormalizedLinearInterpolation2(Quaternion start, Quaternion end, float amount)
{
var q = end;
var cosine = (start | q);
if (cosine < 0) q = -q;
var k = (1 - Math.Abs(cosine)) * 0.4669269f;
var s = 2 * k * amount * amount * amount - 3 * k * amount * amount + (1 + k) * amount;
this.V.X = start.V.X * (1.0f - s) + q.V.X * s;
this.V.Y = start.V.Y * (1.0f - s) + q.V.Y * s;
this.V.Z = start.V.Z * (1.0f - s) + q.V.Z * s;
this.W = start.W * (1.0f - s) + q.W * s;
this.Normalize();
}
/// <summary>
/// Determines whether this quaternion is sufficiently close to another one.
/// </summary>
/// <param name="q"> Another quaternion.</param>
/// <param name="epsilon">Precision of comparison.</param>
/// <returns>
/// True, if this quaternion represents rotation equivalent to one represented by
/// another quaternion.
/// </returns>
public bool IsEquivalent(Quaternion q, float epsilon = 0.05f)
{
var p = -q;
bool t0 = (Math.Abs(this.V.X - q.V.X) <= epsilon) && (Math.Abs(this.V.Y - q.V.Y) <= epsilon) && (Math.Abs(this.V.Z - q.V.Z) <= epsilon) && (Math.Abs(this.W - q.W) <= epsilon);
bool t1 = (Math.Abs(this.V.X - p.V.X) <= epsilon) && (Math.Abs(this.V.Y - p.V.Y) <= epsilon) && (Math.Abs(this.V.Z - p.V.Z) <= epsilon) && (Math.Abs(this.W - p.W) <= epsilon);
t0 |= t1;
return t0;
}
/// <summary>
/// Sets this quaternion to result of normalized linear interpolation.
/// </summary>
/// <param name="start"> Starting quaternion in interpolation.</param>
/// <param name="end"> Ending quaternion in interpolation.</param>
/// <param name="amount">
/// Number between 0 and 1 that sets position of resulting quaternion between
/// <paramref name="start"/> and <paramref name="end"/>.
/// </param>
public void NormalizedLinearInterpolation(Quaternion start, Quaternion end, float amount)
{
var q = end;
if ((start | q) < 0) { q = -q; }
var vDiff = q.V - start.V;
this.V = start.V + (vDiff * amount);
this.W = start.W + ((q.W - start.W) * amount);
this.Normalize();
}
/// <summary>
/// Creates quaternion using spherical linear interpolation algorithm.
/// </summary>
/// <param name="start"> Starting position for interpolation.</param>
/// <param name="end"> Ending position for interpolation.</param>
/// <param name="amount">
/// Value between 0 and 1 that defines "distance" between required quaternion and
/// given two.
/// </param>
/// <returns>
/// Quaternion that represents rotation between rotations defined by given two
/// quaternions.
/// </returns>
public static Quaternion CreateSphericalLinearInterpolation(Quaternion start, Quaternion end, float amount)
{
var q = new Quaternion();
q.SphericalLinearInterpolation(start, end, amount);
return q;
}
/// <summary>
/// </summary>
/// <param name="start"> </param>
/// <param name="end"> </param>
/// <param name="amount"></param>
public void ExpSlerp(Quaternion start, Quaternion end, float amount)
{
var q = end;
if ((start | q) < 0) { q = -q; }
this = start * MathHelpers.Exp(MathHelpers.Log(!start * q) * amount);
}
/// <summary>
/// Creates new quaternion that represents rotation around Z-axis.
/// </summary>
/// <param name="r">Angle of rotation in radians.</param>
/// <returns>Quaternion that represents rotation around Z-axis.</returns>
public static Quaternion CreateRotationAroundZ(float r)
{
var q = new Quaternion();
q.SetRotationAroundZ(r);
return q;
}
/// <summary>
/// Creates quaternion that represents rotation from first vector two the sector
/// via shortest arc.
/// </summary>
/// <param name="one"> First vector.</param>
/// <param name="two"> Second vector.</param>
/// <param name="fallbackAxis">
/// Axis to use to represent 180 degrees rotation when turns out, that two vectors
/// are collinear about point in opposite directions.
/// </param>
/// <returns>
/// Quaternion that represents rotation from first vector two the sector via
/// shortest arc.
/// </returns>
public static Quaternion CreateRotationFrom2Vectors(Vector3 one, Vector3 two, Vector3 fallbackAxis = new Vector3())
{
Quaternion q = new Quaternion();
q.SetRotationFrom2Vectors(one, two, fallbackAxis);
return q;
}
/// <summary>
/// Creates new quaternion that represents rotation by specified angle around
/// given axis.
/// </summary>
/// <param name="cosha">Cosine of angle of rotation.</param>
/// <param name="sinha">Sine of angle of rotation.</param>
/// <param name="axis">
/// <see cref="Vector3"/> that represents an axis of rotation.
/// </param>
/// <returns>
/// Quaternion that represents rotation by specified angle around given axis.
/// </returns>
public static Quaternion CreateRotationAngleAxis(float cosha, float sinha, Vector3 axis)
{
var q = new Quaternion();
q.SetRotationAngleAxis(cosha, sinha, axis);
return q;
}
/// <summary>
/// Creates new quaternion that represents rotation defined by Euler angles.
/// </summary>
/// <param name="angle">Euler angles of rotation in radians.</param>
/// <returns>
/// Quaternion that represents rotation defined by Euler angles.
/// </returns>
public static Quaternion CreateRotationAroundXYZAxes(Vector3 angle)
{
var q = new Quaternion();
q.SetRotationAroundXYZAxes(angle);
return q;
}
/// <summary>
/// Creates quaternion using different normalized linear interpolation algorithm.
/// </summary>
/// <param name="start"> Starting position for interpolation.</param>
/// <param name="end"> Ending position for interpolation.</param>
/// <param name="amount">
/// Value between 0 and 1 that defines "distance" between required quaternion and
/// given two.
/// </param>
/// <returns>
/// Quaternion that represents rotation between rotations defined by given two
/// quaternions.
/// </returns>
public static Quaternion CreateNormalizedLinearInterpolation2(Quaternion start, Quaternion end, float amount)
{
var q = new Quaternion();
q.NormalizedLinearInterpolation2(start, end, amount);
return q;
}
/// <summary>
/// Creates new quaternion that represents rotation by specified angle around
/// given axis.
/// </summary>
/// <param name="rad"> Angle in radians.</param>
/// <param name="axis">
/// <see cref="Vector3"/> that represents an axis of rotation.
/// </param>
/// <returns>
/// Quaternion that represents rotation by specified angle around given axis.
/// </returns>
public static Quaternion CreateRotationAngleAxis(float rad, Vector3 axis)
{
var q = new Quaternion();
q.SetRotationAngleAxis(rad, axis);
return q;
}
/// <summary>
/// </summary>
/// <param name="start"> </param>
/// <param name="end"> </param>
/// <param name="amount"></param>
/// <returns></returns>
public static Quaternion CreateExpSlerp(Quaternion start, Quaternion end, float amount)
{
var q = new Quaternion();
q.ExpSlerp(start, end, amount);
return q;
}
/// <summary>
/// Sets this quaternion to result of spherical linear interpolation.
/// </summary>
/// <param name="start"> Starting quaternion in interpolation.</param>
/// <param name="end"> Ending quaternion in interpolation.</param>
/// <param name="amount">
/// Number between 0 and 1 that sets position of resulting quaternion between
/// <paramref name="start"/> and <paramref name="end"/>.
/// </param>
public void SphericalLinearInterpolation(Quaternion start, Quaternion end, float amount)
{
var p = start;
var q = end;
var q2 = new Quaternion();
var cosine = (p | q);
if (cosine < 0.0f) { cosine = -cosine; q = -q; } // take shortest arc
if (cosine > 0.9999f)
{
this.NormalizedLinearInterpolation(p, q, amount);
return;
}
// from now on, a division by 0 is not possible any more
q2.W = q.W - p.W * cosine;
q2.V.X = q.V.X - p.V.X * cosine;
q2.V.Y = q.V.Y - p.V.Y * cosine;
q2.V.Z = q.V.Z - p.V.Z * cosine;
var sine = Math.Sqrt(q2 | q2);
double s, c;
MathHelpers.SinCos(Math.Atan2(sine, cosine) * amount, out s, out c);
this.W = (float)(p.W * c + q2.W * s / sine);
this.V.X = (float)(p.V.X * c + q2.V.X * s / sine);
this.V.Y = (float)(p.V.Y * c + q2.V.Y * s / sine);
this.V.Z = (float)(p.V.Z * c + q2.V.Z * s / sine);
}
/// <summary>
/// Creates new instance of type <see cref="QuaternionTranslation"/>.
/// </summary>
/// <param name="m">
/// <see cref="Matrix34"/> that represents both translation and orientation.
/// </param>
public QuaternionTranslation(Matrix34 m)
{
this.Q = new Quaternion(m);
this.T = m.Translation;
}
/// <summary>
/// Creates new instance of type <see cref="QuaternionTranslation"/>.
/// </summary>
/// <param name="t"><see cref="Vector3"/> that represents translation.</param>
/// <param name="q"><see cref="Quaternion"/> that represents orientation.</param>
public QuaternionTranslation(Vector3 t, Quaternion q)
{
this.Q = q;
this.T = t;
}
/// <summary>
/// Inverts rotation and translation this object represents.
/// </summary>
public void Invert()
{
this.T = -this.T * this.Q;
this.Q = !this.Q;
}
