Calculate Euler Angles from BlockReference.Transform - Matrix3d

[Anonymous]

I'm having trouble calculating the Euler Angles (Yaw, Pitch, Roll) for a given block's Transform member. Assuming WCS = Matrix3d.Identity and ignoring UCS I have the following code for calculating the rotation about each axis stored in the Matrix3d:

    Public Shared Function CalculateEulerAngles(Transform As Matrix3d) As Vector3d
        Dim x As Double = Math.Atan2(Transform.ElementAt(3, 2), Transform.ElementAt(3, 3))
        Dim y As Double = Math.Atan2(-Transform.ElementAt(3, 1), Math.Sqrt(Transform.ElementAt(3, 2) ^ 2 + Transform.ElementAt(3, 3) ^ 2))
        Dim z As Double = Math.Atan2(Transform.ElementAt(2, 1), Transform.ElementAt(1, 1))
        Return New Vector3d(x, y, z)
    End Function

This calculation is obviously incorrect from the results I receive. 

Kevin

[Alexander.Rivilis]

It is look like you using wrong algorithm. Maybe this algorithm help you: http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf

But if matrix has nonuniform scaling that algorithm also wrong.

[_gile]

Hi,

Assuming you're trying to get the rotation angles about Z, Y and X axis (respectively yaw, pitch and roll) as defined in the specific Euler angles definition called Tait-Bryan angles using the z-y'-x" convention (see here and there), in other words, the angles along the X, Y and Z axes of an object that would have undergone rotations, first along the X axis, then along the Y axis and finally along the Z axis.

Maybe you can get some inspiration from this little class.

using Autodesk.AutoCAD.Geometry;
using System;
using static System.Math;

namespace Gile.AutoCAD.Geometry
{
    /// <summary>
    /// Describes a 3d rotation with Yaw Pitch and Roll
    /// (i.e. a specific Euler angles called Tait-Ryan angles with z-y'-x" convention).
    /// </summary>
    public class TaitBryan
    {
        /// <summary>
        /// Get the transformation matrix.
        /// </summary>
        public Matrix3d Transform { get; private set; }

        /// <summary>
        /// Gets the rotation matrix about Z axis.
        /// </summary>
        public Matrix3d Yaw { get; private set; }

        /// <summary>
        /// Gets the rotation matrix about Y axis.
        /// </summary>
        public Matrix3d Pitch { get; private set; }

        /// <summary>
        /// Gets the rotation matrix about X axis.
        /// </summary>
        public Matrix3d Roll { get; private set; }

        /// <summary>
        /// Gets the rotation angle about Z axis.
        /// </summary>
        public double Alpha { get; private set; }

        /// <summary>
        /// Gets the rotation angle about Y axis.
        /// </summary>
        public double Beta { get; private set; }

        /// <summary>
        /// Gets the rotation angle about X axis.
        /// </summary>
        public double Gamma { get; private set; }

        /// <summary>
        /// Create a new intance of TaitBryan.
        /// </summary>
        /// <param name="xform">Transformation matrix.</param>
        public TaitBryan(Matrix3d xform)
        {
            if (!xform.IsUniscaledOrtho())
                throw new ArgumentException("Non uniscaled ortho matrix.");

            Transform = xform;
            Alpha = Atan2(xform[1, 0], xform[0, 0]);
            Beta = Atan2(-xform[2, 0], Sqrt(xform[2, 1] * xform[2, 1] + xform[2, 2] * xform[2, 2]));
            Gamma = Atan2(xform[2, 1], xform[2, 2]);
            Yaw = Matrix3d.Rotation(Alpha, Vector3d.ZAxis, Point3d.Origin);
            Pitch = Matrix3d.Rotation(Beta, Vector3d.YAxis, Point3d.Origin);
            Roll = Matrix3d.Rotation(Gamma, Vector3d.XAxis, Point3d.Origin);
        }

        /// <summary>
        /// Create a new intance of TaitBryan.
        /// </summary>
        /// <param name="alpha">Rotation angle about Z axis.</param>
        /// <param name="beta">Rotation angle about Y axis.</param>
        /// <param name="gamma"Rotation angle about X axis.></param>
        public TaitBryan(double alpha, double beta, double gamma)
        {
            Alpha = alpha;
            Beta = beta;
            Gamma = gamma;
            Yaw = Matrix3d.Rotation(Alpha, Vector3d.ZAxis, Point3d.Origin);
            Pitch = Matrix3d.Rotation(Beta, Vector3d.YAxis, Point3d.Origin);
            Roll = Matrix3d.Rotation(Gamma, Vector3d.XAxis, Point3d.Origin);
            Transform = Yaw * Pitch * Roll;
        }
    }
}

[_gile · Accepted solution]

Oops!...

The link provided by @Alexander.Rivilis made me see I forgot the cases of rotations of 90° or -90° about Y axis which may cause a 'divide by zero' error.

Here's a safer implementation

using Autodesk.AutoCAD.Geometry;
using System;
using static System.Math;

namespace Gile.AutoCAD.Geometry
{
    /// <summary>
    /// Describes a 3d rotation with Yaw Pitch and Roll
    /// (i.e. a specific Euler angles called Tait-Ryan angles with z-y'-x" convention).
    /// </summary>
    public class TaitBryan
    {
        /// <summary>
        /// Get the transformation matrix.
        /// </summary>
        public Matrix3d Transform { get; private set; }

        /// <summary>
        /// Gets the rotation matrix about Z axis.
        /// </summary>
        public Matrix3d Yaw { get; private set; }

        /// <summary>
        /// Gets the rotation matrix about Y axis.
        /// </summary>
        public Matrix3d Pitch { get; private set; }

        /// <summary>
        /// Gets the rotation matrix about X axis.
        /// </summary>
        public Matrix3d Roll { get; private set; }

        /// <summary>
        /// Gets the rotation angle about Z axis.
        /// </summary>
        public double Alpha { get; private set; }

        /// <summary>
        /// Gets the rotation angle about Y axis.
        /// </summary>
        public double Beta { get; private set; }

        /// <summary>
        /// Gets the rotation angle about X axis.
        /// </summary>
        public double Gamma { get; private set; }

        /// <summary>
        /// Create a new intance of TaitBryan.
        /// </summary>
        /// <param name="xform">Transformation matrix.</param>
        public TaitBryan(Matrix3d xform)
        {
            if (!xform.IsUniscaledOrtho())
                throw new ArgumentException("Non uniscaled ortho matrix.");

            Transform = xform;
            Beta = -Asin(xform[2, 0] / xform.GetScale());
            if (Abs(Beta - PI * 0.5) < 1e-7)
            {
                Beta = PI * 0.5;
                Alpha = Atan2(xform[1, 2], xform[1, 1]);
                Gamma = 0.0;
            }
            else if (Abs(Beta + PI * 0.5) < 1e-7)
            {
                Beta = -PI * 0.5;
                Alpha = Atan2(-xform[1, 2], xform[1, 1]);
                Gamma = 0.0;
            }
            else
            {
                Alpha = Atan2(xform[1, 0], xform[0, 0]);
                Gamma = Atan2(xform[2, 1], xform[2, 2]);
            }
            Yaw = Matrix3d.Rotation(Alpha, Vector3d.ZAxis, Point3d.Origin);
            Pitch = Matrix3d.Rotation(Beta, Vector3d.YAxis, Point3d.Origin);
            Roll = Matrix3d.Rotation(Gamma, Vector3d.XAxis, Point3d.Origin);
        }

        /// <summary>
        /// Create a new intance of TaitBryan.
        /// </summary>
        /// <param name="alpha">Rotation angle about Z axis.</param>
        /// <param name="beta">Rotation angle about Y axis.</param>
        /// <param name="gamma"Rotation angle about X axis.></param>
        public TaitBryan(double alpha, double beta, double gamma)
        {
            Alpha = alpha;
            Beta = beta;
            Gamma = gamma;
            Yaw = Matrix3d.Rotation(Alpha, Vector3d.ZAxis, Point3d.Origin);
            Pitch = Matrix3d.Rotation(Beta, Vector3d.YAxis, Point3d.Origin);
            Roll = Matrix3d.Rotation(Gamma, Vector3d.XAxis, Point3d.Origin);
            Transform = Yaw * Pitch * Roll;
        }
    }
}

[_gile]

Here's an implementation which illustrates the diffeence between proper Euler angles (Z-X'-Z") and Tait-Bryan angles (Z-Y'-X").

Abstract base class

using Autodesk.AutoCAD.Geometry;

namespace Gile.AutoCAD.Geometry
{
    /// <summary>
    /// Base class for Euler angles descriptions.
    /// </summary>
    public abstract class EulerAngles
    {
        protected double psi, theta, phi;
        protected Matrix3d xform;

        /// <summary>
        /// Gets the precession angle.
        /// </summary>
        public double Alpha => psi;

        /// <summary>
        /// Gets the nutation angle.
        /// </summary>
        public double Beta => theta;

        /// <summary>
        /// Gets the spin angle.
        /// </summary>
        public double Gamma => phi;

        /// <summary>
        /// Get the transformation matrix.
        /// </summary>
        public Matrix3d Transform => xform;

        /// <summary>
        /// Base constructor.
        /// </summary>
        /// <param name="transform">Transformation matrix.</param>
        public EulerAngles(Matrix3d transform)
        {
            xform = transform;
        }

        /// <summary>
        /// Base constructor.
        /// </summary>
        /// <param name="alpha">Precession angle.</param>
        /// <param name="beta">Nutation angle.</param>
        /// <param name="gamma">Intrinsic rotation.</param>
        public EulerAngles(double alpha, double beta, double gamma)
        {
            psi = alpha;
            theta = beta;
            phi = gamma;
        }
    }
}

Proper Euler angles.

using Autodesk.AutoCAD.Geometry;
using System;
using static System.Math;

namespace Gile.AutoCAD.Geometry
{
    /// <summary>
    /// Describes a 3d rotation with precession, nutation and intrinsic rotation
    /// (i.e. a proper Euler angles with z-x'-z" convention).
    /// </summary>
    public class ProperEuler : EulerAngles
    {
        /// <summary>
        /// Gets the precession rotation matrix (Z).
        /// </summary>
        public Matrix3d Precession { get; private set; }

        /// <summary>
        /// Gets the nutation rotation matrix (X').
        /// </summary>
        public Matrix3d Nutation { get; private set; }

        /// <summary>
        /// Gets the spin rotation matrix (Z").
        /// </summary>
        public Matrix3d Spin { get; private set; }

        /// <summary>
        /// Create a new intance of ProperEuler.
        /// </summary>
        /// <param name="xform">Transformation matrix.</param>
        public ProperEuler(Matrix3d xform) : base(xform)
        {
            if (!xform.IsUniscaledOrtho())
                throw new ArgumentException("Non uniscaled ortho matrix.");

            theta = Acos(xform[2, 2] / xform.GetScale());
            if (Abs(theta) < 1e-7)
            {
                theta = 0.0;
                psi = Atan2(xform[1, 0], xform[1, 1]);
                phi = 0.0;
            }
            else
            {
                psi = Atan2(xform[0, 2], -xform[1, 2]);
                phi = Atan2(xform[2, 0], xform[2, 1]);
            }
            Precession = Matrix3d.Rotation(Alpha, Vector3d.ZAxis, Point3d.Origin);
            Nutation = Matrix3d.Rotation(Beta, Vector3d.XAxis, Point3d.Origin);
            Spin = Matrix3d.Rotation(Gamma, Vector3d.ZAxis, Point3d.Origin);
        }

        /// <summary>
        /// Create a new intance of ProperEuler.
        /// </summary>
        /// <param name="alpha">Precession angle.</param>
        /// <param name="beta">Nutation angle.</param>
        /// <param name="gamma">Intrinsic rotation angle.</param>
        public ProperEuler(double alpha, double beta, double gamma) : base(alpha, beta, gamma)
        {
            Precession = Matrix3d.Rotation(Alpha, Vector3d.ZAxis, Point3d.Origin);
            Nutation = Matrix3d.Rotation(Beta, Vector3d.XAxis, Point3d.Origin);
            Spin = Matrix3d.Rotation(Gamma, Vector3d.ZAxis, Point3d.Origin);
            xform = Precession * Nutation * Spin;
        }
    }
}

Tait-Bryan angles.

using Autodesk.AutoCAD.Geometry;
using System;
using static System.Math;

namespace Gile.AutoCAD.Geometry
{
    /// <summary>
    /// Describes a 3d rotation with Yaw Pitch and Roll
    /// (i.e. a specific Euler angles called Tait-Ryan angles with z-y'-x" convention).
    /// </summary>
    public class TaitBryan : EulerAngles
    {
        /// <summary>
        /// Gets the yaw rotation matrix (Z).
        /// </summary>
        public Matrix3d Yaw { get; private set; }

        /// <summary>
        /// Gets the pitch rotation matrix (Y').
        /// </summary>
        public Matrix3d Pitch { get; private set; }

        /// <summary>
        /// Gets roll the rotation matrix (X").
        /// </summary>
        public Matrix3d Roll { get; private set; }

        /// <summary>
        /// Create a new intance of TaitBryan.
        /// </summary>
        /// <param name="xform">Transformation matrix.</param>
        public TaitBryan(Matrix3d xform) : base(xform)
        {
            if (!xform.IsUniscaledOrtho())
                throw new ArgumentException("Non uniscaled ortho matrix.");

            theta = -Asin(xform[2, 0] / xform.GetScale());
            if (Abs(theta - PI * 0.5) < 1e-7)
            {
                theta = PI * 0.5;
                psi = Atan2(xform[1, 2], xform[1, 1]);
                phi = 0.0;
            }
            else if (Abs(Beta + PI * 0.5) < 1e-7)
            {
                theta = -PI * 0.5;
                psi = Atan2(-xform[1, 2], xform[1, 1]);
                phi = 0.0;
            }
            else
            {
                psi = Atan2(xform[1, 0], xform[0, 0]);
                phi = Atan2(xform[2, 1], xform[2, 2]);
            }
            Yaw = Matrix3d.Rotation(Alpha, Vector3d.ZAxis, Point3d.Origin);
            Pitch = Matrix3d.Rotation(Beta, Vector3d.YAxis, Point3d.Origin);
            Roll = Matrix3d.Rotation(Gamma, Vector3d.XAxis, Point3d.Origin);
        }

        /// <summary>
        /// Create a new intance of TaitBryan.
        /// </summary>
        /// <param name="alpha">Rotation angle about Z axis.</param>
        /// <param name="beta">Rotation angle about Y axis.</param>
        /// <param name="gamma">Rotation angle about X axis.</param>
        public TaitBryan(double alpha, double beta, double gamma) : base(alpha, beta, gamma)
        {
            Yaw = Matrix3d.Rotation(Alpha, Vector3d.ZAxis, Point3d.Origin);
            Pitch = Matrix3d.Rotation(Beta, Vector3d.YAxis, Point3d.Origin);
            Roll = Matrix3d.Rotation(Gamma, Vector3d.XAxis, Point3d.Origin);
            xform = Yaw * Pitch * Roll;
        }
    }
}

[_gile]

Hi,

Playing a little further with this, i notice that the ProperEuler type makes it easy to establish a relationship between the Euler angle and how AutoCAD exposes a 3d rotation using a vector (Normal) and an angle (Rotation).

So, here's one more new implementation (I removed the (IMHO unsefull) rotation 3d matrices properties).

The base 'EulerAngles' abstract class.

using Autodesk.AutoCAD.Geometry;
using static System.Math;
 
namespace Gile.AutoCAD.Geometry
{
    /// <summary>
    /// Base class for Euler angles defintions.
    /// </summary>
    public abstract class EulerAngles
    {
        protected double psi, theta, phi;
        protected Matrix3d xform;
 
        /// <summary>
        /// Gets the angle alpha (or psi).
        /// </summary>
        public double Alpha => psi;
 
        /// <summary>
        /// Gets the angle beta (or theta).
        /// </summary>
        public double Beta => theta;
 
        /// <summary>
        /// Gets the angle gamma (or phi).
        /// </summary>
        public double Gamma => phi;
 
        /// <summary>
        /// Get the transformation matrix.
        /// </summary>
        public Matrix3d Transform => xform;
 
        /// <summary>
        /// Base constructor.
        /// </summary>
        /// <param name="transform">Transformation matrix.</param>
        public EulerAngles(Matrix3d transform)
        {
            if (!transform.IsUniscaledOrtho())
                throw new System.ArgumentException("Non uniscaled ortho matrix.");
            xform = transform;
        }
 
        /// <summary>
        /// Base constructor.
        /// </summary>
        /// <param name="alpha">Precession angle.</param>
        /// <param name="beta">Nutation angle.</param>
        /// <param name="gamma">Intrinsic rotation.</param>
        public EulerAngles(double alpha, double beta, double gamma)
        {
            psi = Wrap(alpha);
            theta = Wrap(beta);
            phi = Wrap(gamma);
        }
 
        /// <summary>
        /// Equality operator.
        /// </summary>
        /// <param name="x">Left operand.</param>
        /// <param name="y">Right operand.</param>
        /// <returns>true if the values of its operands are equal, false otherwise.</returns>
        public static bool operator ==(EulerAngles x, EulerAngles y) => x.Equals(y);
 
        /// <summary>
        /// Inequality operator.
        /// </summary>
        /// <param name="x">Left operand.</param>
        /// <param name="y">Right operand.</param>
        /// <returns>true if the values of its operands are not equal, false otherwise.</returns>
        public static bool operator !=(EulerAngles x, EulerAngles y) => !x.Equals(y);
 
        /// <summary>
        /// Determines whether the specified object is equal to the current object.
        /// </summary>
        /// <param name="obj">The object to compare with the current object.</param>
        /// <returns>true if the specified object is equal to the current object; otherwise, false.</returns>
        public override bool Equals(object obj)
        {
            var other = obj as EulerAngles;
            return psi == other?.psi && theta == other.theta && phi == other.phi;
        }
 
        /// <summary>
        /// Serves as hash function.
        /// </summary>
        /// <returns>A hash code for the current object.</returns>
        public override int GetHashCode() => psi.GetHashCode() ^ theta.GetHashCode() ^ phi.GetHashCode();
 
        /// <summary>
        /// Returns a string that represents the current object.
        /// </summary>
        /// <returns>A string that represents the current object.</returns>
        public override string ToString() => $"({Alpha}, {Beta}, {Gamma})";
 
        /// <summary>
        /// Return the angle in [0,2*PI) range.
        /// </summary>
        private double Wrap(double angle) => 
            0.0 <= angle && angle < 2 * PI ? angle : Asin(Sin(angle));
    }
}

The 'ProperEuler' class (can also be used with Normal and Rotation)
Transform = Rotation_Z(Alpha) * Rotation_X(Beta) * Rotation_Z(Gamma)

using Autodesk.AutoCAD.Geometry;
using static System.Math;
 
namespace Gile.AutoCAD.Geometry
{
    /// <summary>
    /// Defines the relations between a transformation matrix and Euler angles
    /// (proper Euler angles using z-x'-z" convention).
    /// </summary>
    public class ProperEuler : EulerAngles
    {
        Matrix3d planeToWorld;
 
        /// <summary>
        /// Gets the normal of the plane.
        /// </summary>
        public Vector3d Normal => planeToWorld.CoordinateSystem3d.Zaxis;
 
        /// <summary>
        /// Gets the rotation on the plane;
        /// </summary>
        public double Rotation => phi;
 
        /// <summary>
        /// Create a new intance of ProperEuler.
        /// </summary>
        /// <param name="transform">Transformation matrix.</param>
        public ProperEuler(Matrix3d transform) : base(transform)
        {
            theta = Acos(xform[2, 2] / xform.GetScale());
            if (Abs(theta) < 1e-7)
            {
                theta = 0.0;
                psi = Atan2(xform[1, 0], xform[1, 1]);
                phi = 0.0;
            }
            else
            {
                psi = Atan2(xform[0, 2], -xform[1, 2]);
                phi = Atan2(xform[2, 0], xform[2, 1]);
            }
            planeToWorld = 
                Matrix3d.Rotation(psi, Vector3d.ZAxis, Point3d.Origin) *
                Matrix3d.Rotation(theta, Vector3d.XAxis, Point3d.Origin);
        }
 
        /// <summary>
        /// Create a new intance of ProperEuler.
        /// </summary>
        /// <param name="alpha">Rotation angle around the Z axis.</param>
        /// <param name="beta">Rotation angle around the X' axis.</param>
        /// <param name="gamma">Rotation angle around the Z" axis.</param>
        public ProperEuler(double alpha, double beta, double gamma) : base(alpha, beta, gamma)
        {
            planeToWorld =
                Matrix3d.Rotation(alpha, Vector3d.ZAxis, Point3d.Origin) *
                Matrix3d.Rotation(beta, Vector3d.XAxis, Point3d.Origin);
            xform = planeToWorld * 
                Matrix3d.Rotation(gamma, Vector3d.ZAxis, Point3d.Origin);
        }
 
        /// <summary>
        /// Create a new intance of ProperEuler.
        /// </summary>
        /// <param name="normal">Plane normal.</param>
        /// <param name="rotation">Proper rotation.</param>
        public ProperEuler(Vector3d normal, double rotation)
            : this(Matrix3d.PlaneToWorld(normal) *
                   Matrix3d.Rotation(rotation, Vector3d.ZAxis, Point3d.Origin))
        { }
 
    /// <summary>
    /// Determines whether the specified object is equal to the current object.
    /// </summary>
    /// <param name="obj">The object to compare with the current object.</param>
    /// <returns>true if the specified object is equal to the current object; otherwise, false.</returns>
    public override bool Equals(object obj) => obj is ProperEuler && base.Equals(obj);
 
        /// <summary>
        /// Serves as hash function.
        /// </summary>
        /// <returns>A hash code for the current object.</returns>
        public override int GetHashCode() => base.GetHashCode();
    }
}

The TaitBryan class (yaw,pitch, roll)
Transform = Rotation_Z(Alpha) * Rotation_Y(Beta) * Rotation_X(Gamma)

using Autodesk.AutoCAD.Geometry;
using static System.Math;
 
namespace Gile.AutoCAD.Geometry
{
    /// <summary>
    /// Defines the relations between a transformation matrix and Euler angles
    /// (specific Euler angles called Tait-Ryan angles using z-y'-x" convention).
    /// </summary>
    public class TaitBryan : EulerAngles
    {
        /// <summary>
        /// Create a new intance of TaitBryan.
        /// </summary>
        /// <param name="transform">Transformation matrix.</param>
        public TaitBryan(Matrix3d transform) : base(transform)
        {
            theta = -Asin(xform[2, 0] / xform.GetScale());
            if (Abs(theta - PI * 0.5) < 1e-7)
            {
                theta = PI * 0.5;
                psi = Atan2(xform[1, 2], xform[1, 1]);
                phi = 0.0;
            }
            else if (Abs(Beta + PI * 0.5) < 1e-7)
            {
                theta = -PI * 0.5;
                psi = Atan2(-xform[1, 2], xform[1, 1]);
                phi = 0.0;
            }
            else
            {
                psi = Atan2(xform[1, 0], xform[0, 0]);
                phi = Atan2(xform[2, 1], xform[2, 2]);
            }
        }
 
        /// <summary>
        /// Create a new intance of TaitBryan.
        /// </summary>
        /// <param name="alpha">Rotation angle around the Z axis.</param>
        /// <param name="beta">Rotation angle around the Y' axis.</param>
        /// <param name="gamma">Rotation angle around the X" axis.</param>
        public TaitBryan(double alpha, double beta, double gamma) : base(alpha, beta, gamma)
        {
            xform = 
                Matrix3d.Rotation(alpha, Vector3d.ZAxis, Point3d.Origin) *
                Matrix3d.Rotation(beta, Vector3d.YAxis, Point3d.Origin) *
                Matrix3d.Rotation(gamma, Vector3d.XAxis, Point3d.Origin);
        }
 
        /// <summary>
        /// Determines whether the specified object is equal to the current object.
        /// </summary>
        /// <param name="obj">The object to compare with the current object.</param>
        /// <returns>true if the specified object is equal to the current object; otherwise, false.</returns>
        public override bool Equals(object obj) => obj is TaitBryan && base.Equals(obj);
 
        /// <summary>
        /// Serves as hash function.
        /// </summary>
        /// <returns>A hash code for the current object.</returns>
        public override int GetHashCode() => base.GetHashCode();
    }
}

[Anonymous]

Your code worked perfectly. I thank you from the bottom of my heart :).

Best Regards,

Kevin