/// <summary> /// See http://www.ee.ucr.edu/~farell/AidedNavigation/D_App_Quaternions/Rot2Quat.pdf /// </summary> /// <param name="rm">The rotation matrix</param> private void Diagonal1Solution(RotationMatrix rm) { this.W = (float)(0.5 * Math.Sqrt(1 + rm[0, 0] + rm[1, 1] + rm[2, 2])); this.X = (rm[2, 1] - rm[1, 2]) / (4 * this.W); this.Y = (rm[0, 2] - rm[2, 0]) / (4 * this.W); this.Z = (rm[1, 0] - rm[0, 1]) / (4 * this.W); }
/// <summary> /// Returns the rotation matrix that is in the transformation matrix. /// </summary> /// <returns>The 3x3 rotation matrix which is in the transformation matrix.</returns> public RotationMatrix GetRotation() { RotationMatrix res = new RotationMatrix(); this.SubMatrix(0, 3, 0, 3).CopyTo(res); return(res); }
/// <summary> /// Initializes a new instance of the <see cref="Quaternion"/> class. /// Creates quaternion based equivalent to a rotation matrix. /// </summary> /// <param name="rm">The rotation matrix.</param> public Quaternion(RotationMatrix rm) { float[] methods = { rm[0, 0] + rm[1, 1] + rm[2, 2], rm[0, 0] - rm[1, 1] - rm[2, 2], ((-1 * rm[0, 0]) + rm[1, 1]) - rm[2, 2], ((-1 * rm[0, 0]) - rm[1, 1]) + rm[2, 2] }; switch (methods.ToList().IndexOf(methods.Max())) { case 0: this.Diagonal1Solution(rm); break; case 1: this.Diagonal2Solution(rm); break; case 2: this.Diagonal3Solution(rm); break; case 3: this.Diagonal4Solution(rm); break; } this.EulerAnglesDegree = this.CalcEulerAngles(); }
/// <summary> /// Initializes a new instance of the <see cref="TransformationMatrix"/> class. /// Creates a 4x4 transformation matrix which corresponds to a rotation followed by a translation. /// Sets w to default value 1. /// </summary> /// <param name="xt">X axis translation.</param> /// <param name="yt">Y axis translation.</param> /// <param name="zt">Z axis translation.</param> /// <param name="rm">The rotationmatrix defining the rotation to use.</param> public TransformationMatrix(float xt, float yt, float zt, RotationMatrix rm) : base(4, 4, CreateMatrixArray(xt, yt, zt, 1)) { this.SetSubMatrix(0, 0, rm); }