// Combine two rotations.
// @param q the inner rotation
// @returns the resulting rotation (like this*q for matrices)
public Quaternion Multiply(Quaternion q)
{
// real and img parts of args
float r1 = this._r;
float r2 = q._r;
Vector3D i1 = this.Imaginary();
Vector3D i2 = q.Imaginary();
// Real is product of real, and dot-product of imag
float real_v = (r1 * r2) - (i1 * i2);
// Imag is cross product of imag + real*imag for each
Point3D img = Point3D.VectorProduct(i1, i2);
//Point3D img2 = (img + (i2 * r1) + (i1 * r2));
// Finally, build the result
Quaternion prod = new Quaternion(img.GetX(), img.GetY(), img.GetZ(), real_v);
return(prod);
}
/// <summary>
///
/// </summary>
/// <param name="axis">the axis (doesn't need to be a unit vector) around which to rotate</param>
/// <param name="angle">the rotation angle in radians</param>
public void Set(Point3D axis, float angle)
{
if (Math.Abs(angle) < 1e-8)
{
_x = 0.0f;
_y = 0.0f;
_z = 0.0f;
_r = 1.0f;
}
else
{
float a = angle / 2.0f; // half angle
float s = (float)Math.Sin(a);
float c = (float)Math.Cos(a);
// Make sure the axis is a unit vector
float n = 1.0f / axis.Norm();
_x = s * axis.GetX() * n; // half angle multiplied with axis
_y = s * axis.GetY() * n; // half angle multiplied with axis
_z = s * axis.GetZ() * n; // half angle multiplied with axis
_r = c; // real part is cosine of half angle
}
}
/// <summary>
///
/// </summary>
/// <param name="axis">the axis (doesn't need to be a unit vector) around which to rotate</param>
/// <param name="angle">the rotation angle in radians</param>
public void Set(Point3D axis, float angle)
{
if (Math.Abs(angle) < 1e-8)
{
_x = 0.0f;
_y = 0.0f;
_z = 0.0f;
_r = 1.0f;
}
else
{
float a = angle / 2.0f; // half angle
float s = (float)Math.Sin(a);
float c = (float)Math.Cos(a);
// Make sure the axis is a unit vector
float n = 1.0f / axis.Norm();
_x = s * axis.GetX() * n; // half angle multiplied with axis
_y = s * axis.GetY() * n; // half angle multiplied with axis
_z = s * axis.GetZ() * n; // half angle multiplied with axis
_r = c; // real part is cosine of half angle
}
}