Esempio n. 1
0
 /// <summary>
 /// Build a rotation matrix from the specified quaternion.
 /// </summary>
 /// <param name="q">Quaternion to translate.</param>
 /// <returns>A matrix instance.</returns>
 public static Matrix3d CreateFromQuaternion(Quaterniond q)
 {
     CreateFromQuaternion(ref q, out Matrix3d result);
     return(result);
 }
Esempio n. 2
0
        /// <summary>
        /// Returns the rotation component of this instance. Quite slow.
        /// </summary>
        /// <param name="row_normalise">Whether the method should row-normalise (i.e. remove scale from) the Matrix. Pass false if you know it's already normalised.</param>
        public Quaterniond ExtractRotation(bool row_normalise = true)
        {
            var row0 = Row0;
            var row1 = Row1;
            var row2 = Row2;

            if (row_normalise)
            {
                row0 = row0.Normalized();
                row1 = row1.Normalized();
                row2 = row2.Normalized();
            }

            // code below adapted from Blender

            Quaterniond q     = new Quaterniond();
            double      trace = 0.25 * (row0[0] + row1[1] + row2[2] + 1.0);

            if (trace > 0)
            {
                double sq = Math.Sqrt(trace);

                q.W = sq;
                sq  = 1.0 / (4.0 * sq);
                q.X = (row1[2] - row2[1]) * sq;
                q.Y = (row2[0] - row0[2]) * sq;
                q.Z = (row0[1] - row1[0]) * sq;
            }
            else if (row0[0] > row1[1] && row0[0] > row2[2])
            {
                double sq = 2.0 * Math.Sqrt(1.0 + row0[0] - row1[1] - row2[2]);

                q.X = 0.25 * sq;
                sq  = 1.0 / sq;
                q.W = (row2[1] - row1[2]) * sq;
                q.Y = (row1[0] + row0[1]) * sq;
                q.Z = (row2[0] + row0[2]) * sq;
            }
            else if (row1[1] > row2[2])
            {
                double sq = 2.0 * Math.Sqrt(1.0 + row1[1] - row0[0] - row2[2]);

                q.Y = 0.25 * sq;
                sq  = 1.0 / sq;
                q.W = (row2[0] - row0[2]) * sq;
                q.X = (row1[0] + row0[1]) * sq;
                q.Z = (row2[1] + row1[2]) * sq;
            }
            else
            {
                double sq = 2.0 * Math.Sqrt(1.0 + row2[2] - row0[0] - row1[1]);

                q.Z = 0.25 * sq;
                sq  = 1.0 / sq;
                q.W = (row1[0] - row0[1]) * sq;
                q.X = (row2[0] + row0[2]) * sq;
                q.Y = (row2[1] + row1[2]) * sq;
            }

            q.Normalize();
            return(q);
        }
Esempio n. 3
0
 /// <summary>
 /// Build a rotation matrix from the specified quaternion.
 /// </summary>
 /// <param name="q">Quaternion to translate.</param>
 /// <param name="result">Matrix result.</param>
 public static void CreateFromQuaternion(ref Quaterniond q, out Matrix3d result)
 {
     q.ToAxisAngle(out Vector3d axis, out double angle);
     CreateFromAxisAngle(axis, angle, out result);
 }
Esempio n. 4
0
 /// <summary>
 /// Transforms a vector by a quaternion rotation.
 /// </summary>
 /// <param name="vec">The vector to transform.</param>
 /// <param name="quat">The quaternion to rotate the vector by.</param>
 /// <returns>The result of the operation.</returns>
 public static Vector2d Transform(Vector2d vec, Quaterniond quat)
 {
     Transform(ref vec, ref quat, out Vector2d result);
     return(result);
 }