/// <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; }