/// <summary>
/// Sets the value of this matrix to the matrix conversion of the
/// (single precision) quaternion argument.
/// </summary>
/// <remarks>
/// Sets the value of this matrix to the matrix conversion of the
/// (single precision) quaternion argument.
/// </remarks>
/// <param name="q1">the quaternion to be converted</param>
public void Set(Quat4d q1)
{
this.m00 = (float)(1.0 - 2.0 * q1.y * q1.y - 2.0 * q1.z * q1.z);
this.m10 = (float)(2.0 * (q1.x * q1.y + q1.w * q1.z));
this.m20 = (float)(2.0 * (q1.x * q1.z - q1.w * q1.y));
this.m01 = (float)(2.0 * (q1.x * q1.y - q1.w * q1.z));
this.m11 = (float)(1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.z * q1.z);
this.m21 = (float)(2.0 * (q1.y * q1.z + q1.w * q1.x));
this.m02 = (float)(2.0 * (q1.x * q1.z + q1.w * q1.y));
this.m12 = (float)(2.0 * (q1.y * q1.z - q1.w * q1.x));
this.m22 = (float)(1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.y * q1.y);
}
/// <summary>
/// Sets the value of this quaternion to the normalized value
/// of quaternion q1.
/// </summary>
/// <remarks>
/// Sets the value of this quaternion to the normalized value
/// of quaternion q1.
/// </remarks>
/// <param name="q1">the quaternion to be normalized.</param>
public void Normalize(Quat4d q1)
{
double norm;
norm = (q1.x * q1.x + q1.y * q1.y + q1.z * q1.z + q1.w * q1.w);
if (norm > 0.0)
{
norm = 1.0 / Math.Sqrt(norm);
this.x = norm * q1.x;
this.y = norm * q1.y;
this.z = norm * q1.z;
this.w = norm * q1.w;
}
else
{
this.x = 0.0;
this.y = 0.0;
this.z = 0.0;
this.w = 0.0;
}
}
/// <summary>Constructs and initializes a Quat4d from the specified Quat4d.</summary>
/// <remarks>Constructs and initializes a Quat4d from the specified Quat4d.</remarks>
/// <param name="q1">the Quat4d containing the initialization x y z w data</param>
public Quat4d(Quat4d q1)
: base(q1)
{
}
/// <summary>
/// Multiplies quaternion q1 by the inverse of quaternion q2 and places
/// the value into this quaternion.
/// </summary>
/// <remarks>
/// Multiplies quaternion q1 by the inverse of quaternion q2 and places
/// the value into this quaternion. The value of both argument quaternions
/// is preservered (this = q1 * q2^-1).
/// </remarks>
/// <param name="q1">the first quaternion</param>
/// <param name="q2">the second quaternion</param>
public void MulInverse(Quat4d q1, Quat4d q2)
{
Quat4d tempQuat = new Quat4d(q2);
tempQuat.Inverse();
this.Mul(q1, tempQuat);
}
/// <summary>
/// Multiplies this quaternion by the inverse of quaternion q1 and places
/// the value into this quaternion.
/// </summary>
/// <remarks>
/// Multiplies this quaternion by the inverse of quaternion q1 and places
/// the value into this quaternion. The value of the argument quaternion
/// is preserved (this = this * q^-1).
/// </remarks>
/// <param name="q1">the other quaternion</param>
public void MulInverse(Quat4d q1)
{
Quat4d tempQuat = new Quat4d(q1);
tempQuat.Inverse();
this.Mul(tempQuat);
}
/// <summary>
/// Sets the value of this quaternion to the quaternion product of
/// quaternions q1 and q2 (this = q1 * q2).
/// </summary>
/// <remarks>
/// Sets the value of this quaternion to the quaternion product of
/// quaternions q1 and q2 (this = q1 * q2).
/// Note that this is safe for aliasing (e.g. this can be q1 or q2).
/// </remarks>
/// <param name="q1">the first quaternion</param>
/// <param name="q2">the second quaternion</param>
public void Mul(Quat4d q1, Quat4d q2)
{
if (this != q1 && this != q2)
{
this.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
this.x = q1.w * q2.x + q2.w * q1.x + q1.y * q2.z - q1.z * q2.y;
this.y = q1.w * q2.y + q2.w * q1.y - q1.x * q2.z + q1.z * q2.x;
this.z = q1.w * q2.z + q2.w * q1.z + q1.x * q2.y - q1.y * q2.x;
}
else
{
double x;
double y;
double w;
w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
x = q1.w * q2.x + q2.w * q1.x + q1.y * q2.z - q1.z * q2.y;
y = q1.w * q2.y + q2.w * q1.y - q1.x * q2.z + q1.z * q2.x;
this.z = q1.w * q2.z + q2.w * q1.z + q1.x * q2.y - q1.y * q2.x;
this.w = w;
this.x = x;
this.y = y;
}
}
/// <summary>
/// Sets the value of this quaternion to the quaternion product of
/// itself and q1 (this = this * q1).
/// </summary>
/// <remarks>
/// Sets the value of this quaternion to the quaternion product of
/// itself and q1 (this = this * q1).
/// </remarks>
/// <param name="q1">the other quaternion</param>
public void Mul(Quat4d q1)
{
double x;
double y;
double w;
w = this.w * q1.w - this.x * q1.x - this.y * q1.y - this.z * q1.z;
x = this.w * q1.x + q1.w * this.x + this.y * q1.z - this.z * q1.y;
y = this.w * q1.y + q1.w * this.y - this.x * q1.z + this.z * q1.x;
this.z = this.w * q1.z + q1.w * this.z + this.x * q1.y - this.y * q1.x;
this.w = w;
this.x = x;
this.y = y;
}
/// <summary>
/// Performs a great circle interpolation between quaternion q1
/// and quaternion q2 and places the result into this quaternion.
/// </summary>
/// <remarks>
/// Performs a great circle interpolation between quaternion q1
/// and quaternion q2 and places the result into this quaternion.
/// </remarks>
/// <param name="q1">the first quaternion</param>
/// <param name="q2">the second quaternion</param>
/// <param name="alpha">the alpha interpolation parameter</param>
public void Interpolate(Quat4d q1, Quat4d q2, double alpha)
{
// From "Advanced Animation and Rendering Techniques"
// by Watt and Watt pg. 364, function as implemented appeared to be
// incorrect. Fails to choose the same quaternion for the double
// covering. Resulting in change of direction for rotations.
// Fixed function to negate the first quaternion in the case that the
// dot product of q1 and this is negative. Second case was not needed.
double dot;
double s1;
double s2;
double om;
double sinom;
dot = q2.x * q1.x + q2.y * q1.y + q2.z * q1.z + q2.w * q1.w;
if (dot < 0)
{
// negate quaternion
q1.x = -q1.x;
q1.y = -q1.y;
q1.z = -q1.z;
q1.w = -q1.w;
dot = -dot;
}
if ((1.0 - dot) > Eps)
{
om = Math.Acos(dot);
sinom = Math.Sin(om);
s1 = Math.Sin((1.0 - alpha) * om) / sinom;
s2 = Math.Sin(alpha * om) / sinom;
}
else
{
s1 = 1.0 - alpha;
s2 = alpha;
}
w = s1 * q1.w + s2 * q2.w;
x = s1 * q1.x + s2 * q2.x;
y = s1 * q1.y + s2 * q2.y;
z = s1 * q1.z + s2 * q2.z;
}
/// <summary>Sets the value of this quaternion to quaternion inverse of quaternion q1.
/// </summary>
/// <remarks>Sets the value of this quaternion to quaternion inverse of quaternion q1.
/// </remarks>
/// <param name="q1">the quaternion to be inverted</param>
public void Inverse(Quat4d q1)
{
double norm;
norm = 1.0 / (q1.w * q1.w + q1.x * q1.x + q1.y * q1.y + q1.z * q1.z);
this.w = norm * q1.w;
this.x = -norm * q1.x;
this.y = -norm * q1.y;
this.z = -norm * q1.z;
}
/// <summary>
/// Sets the rotational component (upper 3x3) of this matrix to the
/// matrix equivalent values of the quaternion argument; the other
/// elements of this matrix are unchanged; a singular value
/// decomposition is performed on this object's upper 3x3 matrix to
/// factor out the scale, then this object's upper 3x3 matrix components
/// are replaced by the matrix equivalent of the quaternion,
/// and then the scale is reapplied to the rotational components.
/// </summary>
/// <remarks>
/// Sets the rotational component (upper 3x3) of this matrix to the
/// matrix equivalent values of the quaternion argument; the other
/// elements of this matrix are unchanged; a singular value
/// decomposition is performed on this object's upper 3x3 matrix to
/// factor out the scale, then this object's upper 3x3 matrix components
/// are replaced by the matrix equivalent of the quaternion,
/// and then the scale is reapplied to the rotational components.
/// </remarks>
/// <param name="q1">the quaternion that specifies the rotation</param>
public void SetRotation(Quat4d q1)
{
double[] tmp_rot = new double[9];
// scratch matrix
double[] tmp_scale = new double[3];
// scratch matrix
GetScaleRotate(tmp_scale, tmp_rot);
m00 = (float)((1.0f - 2.0f * q1.y * q1.y - 2.0f * q1.z * q1.z) * tmp_scale[0]);
m10 = (float)((2.0f * (q1.x * q1.y + q1.w * q1.z)) * tmp_scale[0]);
m20 = (float)((2.0f * (q1.x * q1.z - q1.w * q1.y)) * tmp_scale[0]);
m01 = (float)((2.0f * (q1.x * q1.y - q1.w * q1.z)) * tmp_scale[1]);
m11 = (float)((1.0f - 2.0f * q1.x * q1.x - 2.0f * q1.z * q1.z) * tmp_scale[1]);
m21 = (float)((2.0f * (q1.y * q1.z + q1.w * q1.x)) * tmp_scale[1]);
m02 = (float)((2.0f * (q1.x * q1.z + q1.w * q1.y)) * tmp_scale[2]);
m12 = (float)((2.0f * (q1.y * q1.z - q1.w * q1.x)) * tmp_scale[2]);
m22 = (float)((1.0f - 2.0f * q1.x * q1.x - 2.0f * q1.y * q1.y) * tmp_scale[2]);
}
/// <summary>Sets the value of this quaternion to the conjugate of quaternion q1.</summary>
/// <remarks>Sets the value of this quaternion to the conjugate of quaternion q1.</remarks>
/// <param name="q1">the source vector</param>
public void Conjugate(Quat4d q1)
{
this.x = -q1.x;
this.y = -q1.y;
this.z = -q1.z;
this.w = q1.w;
}
/// <summary>
/// Sets the value of this matrix from the rotation expressed
/// by the quaternion q1, the translation t1, and the scale s.
/// </summary>
/// <remarks>
/// Sets the value of this matrix from the rotation expressed
/// by the quaternion q1, the translation t1, and the scale s.
/// </remarks>
/// <param name="q1">the rotation expressed as a quaternion</param>
/// <param name="t1">the translation</param>
/// <param name="s">the scale value</param>
public void Set(Quat4d q1, Vector3d t1, double s)
{
this.m00 = (float)(s * (1.0 - 2.0 * q1.y * q1.y - 2.0 * q1.z * q1.z));
this.m10 = (float)(s * (2.0 * (q1.x * q1.y + q1.w * q1.z)));
this.m20 = (float)(s * (2.0 * (q1.x * q1.z - q1.w * q1.y)));
this.m01 = (float)(s * (2.0 * (q1.x * q1.y - q1.w * q1.z)));
this.m11 = (float)(s * (1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.z * q1.z));
this.m21 = (float)(s * (2.0 * (q1.y * q1.z + q1.w * q1.x)));
this.m02 = (float)(s * (2.0 * (q1.x * q1.z + q1.w * q1.y)));
this.m12 = (float)(s * (2.0 * (q1.y * q1.z - q1.w * q1.x)));
this.m22 = (float)(s * (1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.y * q1.y));
this.m03 = (float)t1.x;
this.m13 = (float)t1.y;
this.m23 = (float)t1.z;
this.m30 = (float)0.0;
this.m31 = (float)0.0;
this.m32 = (float)0.0;
this.m33 = (float)1.0;
}
/// <summary>
/// Sets the value of this axis-angle to the rotational equivalent
/// of the passed quaternion.
/// </summary>
/// <remarks>
/// Sets the value of this axis-angle to the rotational equivalent
/// of the passed quaternion.
/// If the specified quaternion has no rotational component, the value
/// of this AxisAngle4d is set to an angle of 0 about an axis of (0,1,0).
/// </remarks>
/// <param name="q1">the Quat4d</param>
public void Set(Quat4d q1)
{
double mag = q1.x * q1.x + q1.y * q1.y + q1.z * q1.z;
if (mag > Eps)
{
mag = Math.Sqrt(mag);
double invMag = 1.0 / mag;
x = q1.x * invMag;
y = q1.y * invMag;
z = q1.z * invMag;
angle = 2.0 * Math.Atan2(mag, q1.w);
}
else
{
x = 0.0f;
y = 1.0f;
z = 0.0f;
angle = 0f;
}
}
/// <summary>
/// Performs an SVD normalization of q1 matrix in order to acquire
/// the normalized rotational component; the values are placed into
/// the Quat4d parameter.
/// </summary>
/// <remarks>
/// Performs an SVD normalization of q1 matrix in order to acquire
/// the normalized rotational component; the values are placed into
/// the Quat4d parameter.
/// </remarks>
/// <param name="q1">the quaternion into which the rotation component is placed</param>
public void Get(Quat4d q1)
{
double[] tmp_rot = new double[9];
// scratch matrix
double[] tmp_scale = new double[3];
// scratch matrix
GetScaleRotate(tmp_scale, tmp_rot);
double ww;
ww = 0.25 * (1.0 + tmp_rot[0] + tmp_rot[4] + tmp_rot[8]);
if (!((ww < 0 ? -ww : ww) < 1.0e-30))
{
q1.w = Math.Sqrt(ww);
ww = 0.25 / q1.w;
q1.x = (tmp_rot[7] - tmp_rot[5]) * ww;
q1.y = (tmp_rot[2] - tmp_rot[6]) * ww;
q1.z = (tmp_rot[3] - tmp_rot[1]) * ww;
return;
}
q1.w = 0.0f;
ww = -0.5 * (tmp_rot[4] + tmp_rot[8]);
if (!((ww < 0 ? -ww : ww) < 1.0e-30))
{
q1.x = Math.Sqrt(ww);
ww = 0.5 / q1.x;
q1.y = tmp_rot[3] * ww;
q1.z = tmp_rot[6] * ww;
return;
}
q1.x = 0.0;
ww = 0.5 * (1.0 - tmp_rot[8]);
if (!((ww < 0 ? -ww : ww) < 1.0e-30))
{
q1.y = Math.Sqrt(ww);
q1.z = tmp_rot[7] / (2.0 * q1.y);
return;
}
q1.y = 0.0;
q1.z = 1.0;
}
/// <summary>
/// Sets the value of this matrix to the matrix conversion of the
/// (double precision) quaternion argument.
/// </summary>
/// <remarks>
/// Sets the value of this matrix to the matrix conversion of the
/// (double precision) quaternion argument.
/// </remarks>
/// <param name="q1">the quaternion to be converted</param>
public void Set(Quat4d q1)
{
this.m00 = (1.0 - 2.0 * q1.y * q1.y - 2.0 * q1.z * q1.z);
this.m10 = (2.0 * (q1.x * q1.y + q1.w * q1.z));
this.m20 = (2.0 * (q1.x * q1.z - q1.w * q1.y));
this.m01 = (2.0 * (q1.x * q1.y - q1.w * q1.z));
this.m11 = (1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.z * q1.z);
this.m21 = (2.0 * (q1.y * q1.z + q1.w * q1.x));
this.m02 = (2.0 * (q1.x * q1.z + q1.w * q1.y));
this.m12 = (2.0 * (q1.y * q1.z - q1.w * q1.x));
this.m22 = (1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.y * q1.y);
this.m03 = 0.0;
this.m13 = 0.0;
this.m23 = 0.0;
this.m30 = 0.0;
this.m31 = 0.0;
this.m32 = 0.0;
this.m33 = 1.0;
}
/// <summary>
/// Constructs and initializes a Matrix4d from the quaternion,
/// translation, and scale values; the scale is applied only to the
/// rotational components of the matrix (upper 3x3) and not to the
/// translational components.
/// </summary>
/// <remarks>
/// Constructs and initializes a Matrix4d from the quaternion,
/// translation, and scale values; the scale is applied only to the
/// rotational components of the matrix (upper 3x3) and not to the
/// translational components.
/// </remarks>
/// <param name="q1">the quaternion value representing the rotational component</param>
/// <param name="t1">the translational component of the matrix</param>
/// <param name="s">the scale value applied to the rotational components</param>
public Matrix4d(Quat4d q1, Vector3d t1, double s)
{
m00 = s * (1.0 - 2.0 * q1.y * q1.y - 2.0 * q1.z * q1.z);
m10 = s * (2.0 * (q1.x * q1.y + q1.w * q1.z));
m20 = s * (2.0 * (q1.x * q1.z - q1.w * q1.y));
m01 = s * (2.0 * (q1.x * q1.y - q1.w * q1.z));
m11 = s * (1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.z * q1.z);
m21 = s * (2.0 * (q1.y * q1.z + q1.w * q1.x));
m02 = s * (2.0 * (q1.x * q1.z + q1.w * q1.y));
m12 = s * (2.0 * (q1.y * q1.z - q1.w * q1.x));
m22 = s * (1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.y * q1.y);
m03 = t1.x;
m13 = t1.y;
m23 = t1.z;
m30 = 0.0;
m31 = 0.0;
m32 = 0.0;
m33 = 1.0;
}
