/// <summary>
/// Multiplies two quaternions, storing the result in out. The resulting
/// quaternion will have the same effect as the combination of p and q,
/// ie. when applied to a point, (point * out) = ((point * p) * q). Any
/// number of rotations can be concatenated in this way. Note that quaternion
/// multiplication is not commutative, ie. quat_mul(p, q) != quat_mul(q, p).
/// </summary>
public Quaternion Multiply (Quaternion q)
{
Quaternion ret;
/* qp = ww' - v.v' + vxv' + wv' + w'v */
/* w" = ww' - xx' - yy' - zz' */
ret.w = (w * q.w) - (x * q.x) - (y * q.y) - (z * q.z);
/* x" = wx' + xw' + yz' - zy' */
ret.x = (w * q.x) + (x * q.w) + (y * q.z) -(z * q.y);
/* y" = wy' + yw' + zx' - xz' */
ret.y = (w * q.y) + (y * q.w) + (z * q.x) - (x * q.z);
/* z" = wz' + zw' + xy' - yx' */
ret.z = w * q.z + z * q.w + x * q.y - y * q.x;
return ret;
}
/// <summary>
/// Constructs a quaternion that represents a rotation between 'from' and
/// 'to'. The argument 't' can be anything between 0 and 1 and represents
/// where between from and to the result will be. 0 returns 'from', 1
/// returns 'to', and 0.5 will return a rotation exactly in between. The
/// result is copied to 'out'.
///
/// The variable 'how' can be any one of the following:
///
/// qshort - This equivalent quat_interpolate, the rotation will be less than 180 degrees
/// qlong - rotation will be greater than 180 degrees
/// qcw - rotation will be clockwise when viewed from above
/// qccw - rotation will be counterclockwise when viewed from above
/// quser - the quaternions are interpolated exactly as given
/// </summary>
public static Quaternion Slerp(Quaternion from, Quaternion to, float t, SlerpType how)
{
Quaternion q;
Quaternion to2;
double angle;
double cos_angle;
double scale_from;
double scale_to;
double sin_angle;
cos_angle = (from.x * to.x) + (from.y * to.y) + (from.z * to.z) + (from.w * to.w);
if (((how == SlerpType.qshort) && (cos_angle < 0.0)) ||
((how == SlerpType.qlong) && (cos_angle > 0.0)) ||
((how == SlerpType.qcw) && (from.w > to.w)) ||
((how == SlerpType.qccw) && (from.w < to.w)))
{
cos_angle = -cos_angle;
to2.w = -to.w;
to2.x = -to.x;
to2.y = -to.y;
to2.z = -to.z;
}
else
to2 = to;
if ((1.0 - Math.Abs(cos_angle)) > Util.Epsilon)
{
/* spherical linear interpolation (SLERP) */
angle = Math.Acos(cos_angle);
sin_angle = Math.Sin(angle);
scale_from = Math.Sin((1.0 - t) * angle) / sin_angle;
scale_to = Math.Sin(t * angle) / sin_angle;
}
else {
/* to prevent divide-by-zero, resort to linear interpolation */
scale_from = 1.0 - t;
scale_to = t;
}
q.w = (float)((scale_from * from.w) + (scale_to * to2.w));
q.x = (float)((scale_from * from.x) + (scale_to * to2.x));
q.y = (float)((scale_from * from.y) + (scale_to * to2.y));
q.z = (float)((scale_from * from.z) + (scale_to * to2.z));
return q;
}