Author Topic: Euler angles from transformation matrix?  (Read 2037 times)

0 Members and 1 Guest are viewing this topic.

kaefer

  • Swamp Rat
  • Posts: 572
Euler angles from transformation matrix?
« on: September 02, 2011, 05:18:00 am »
There's been a contribution to the Autodesk AutoCAD .NET Discussion Group which is based on an Inventor matrix, using the arccos function. I'm suspecting that Inventor, missing the translational part of the matrix, keeps additional constraints that make such calculations possible. Whatever the reason, I wasn't able to port that approach to AutoCAD.

Now here's a naive alternative, which works at least for cartesian rotation (along one axis). Has anybody a better idea?

Code: [Select]
        let ent = tr.GetObject(per.ObjectId, OpenMode.ForRead) :?> Entity
        let cs = ent.CompoundObjectTransform.CoordinateSystem3d
        let a = cs.Yaxis.GetAngleTo(Vector3d.ZAxis, Vector3d.XAxis) - pi2
        let b = -cs.Zaxis.GetAngleTo(Vector3d.XAxis, Vector3d.YAxis) + pi2
        let c = -cs.Xaxis.GetAngleTo(Vector3d.YAxis, Vector3d.ZAxis) + pi2
        ed.WriteMessage("\na: {0}, b: {1}, c: {2} ", a, b, c )



kaefer

  • Swamp Rat
  • Posts: 572
Re: Euler angles from transformation matrix?
« Reply #1 on: September 02, 2011, 06:01:09 pm »
I'm suspecting that Inventor, missing the translational part of the matrix, keeps additional constraints

More trivially, Inventor.Matrix indexer is 1-based. Besides, I've been advised to apply the fishmonger's treatment: Remove scales.

Here's a loose translation of the original VB, with test command, in C#.

Code: [Select]
        [CommandMethod("CalculateRotationAngles")]
        public void CalculateRotationAngleCommand()
        {
            Document doc = AcadApp.DocumentManager.MdiActiveDocument;
            Editor ed = doc.Editor;
            PromptEntityOptions peo = new PromptEntityOptions("Select BlockReference");
            peo.SetRejectMessage("BlockReference only");
            peo.AddAllowedClass(typeof(BlockReference), false);
            PromptEntityResult per = ed.GetEntity(peo);
            if(per.Status != PromptStatus.OK) return;
            using(Transaction tr = doc.Database.TransactionManager.StartTransaction())
            {
                BlockReference bref = (BlockReference)tr.GetObject(per.ObjectId, OpenMode.ForRead);
                double[] rot = CalculateRotationAngles(bref.BlockTransform);
                ed.WriteMessage("\na: {0}, b: {1}, c: {2} ", rot[0], rot[1], rot[2]);
                tr.Commit();
            }
        }
        double Acos(double value)
        {
            return value >= 1 ? 0 : value <= -1 ? Math.PI : Math.Acos(value);
        }
        double[] CalculateRotationAngles(Matrix3d oMatrix)
        {
            oMatrix = Scale3d.RemoveScale(oMatrix).Inverse().GetMatrix() * oMatrix;

            // Original VB for Inventor, posted by philippe.leefsma; found here:
            // http://forums.autodesk.com/t5/NET/Getting-blocs-amp-solids-rotation-angle-in-3D-space/td-p/3140470

            double[] aRotAngles = new double[3];

            // Choose aRotAngles[0] about x which transforms axes[2] onto the x-z plane
            Vector2d v = new Vector2d(oMatrix[1, 2], oMatrix[2, 2]);
            aRotAngles[0] = v.Length <= 0.000001 ? 0 : Math.Sign(v.X) * Acos(v.Y / v.Length);
            oMatrix = Matrix3d.Rotation(aRotAngles[0], Vector3d.XAxis, Point3d.Origin) * oMatrix;
 
            // Choose aRotAngles[1] about y which transforms axes[3] onto the z axis
            aRotAngles[1] = Math.Sign(-oMatrix[0, 2]) * Acos(oMatrix[2, 2]);
            oMatrix = Matrix3d.Rotation(aRotAngles[1], Vector3d.YAxis, Point3d.Origin) * oMatrix;
   
            // Choose aRotAngles[2] about z which transforms axes[0] onto the x axis
            aRotAngles[2] = Math.Sign(-oMatrix[1, 0]) * Acos(oMatrix[0, 0]);
 
            // if you want to get the result in degrees
            const double toDegrees = 180 / Math.PI;
            aRotAngles[0] = aRotAngles[0] * toDegrees;
            aRotAngles[1] = aRotAngles[1] * toDegrees;
            aRotAngles[2] = aRotAngles[2] * toDegrees;
           
            return aRotAngles;
        }


LE3

  • Guest
Re: Euler angles from transformation matrix?
« Reply #2 on: September 02, 2011, 08:19:06 pm »
some links that might help:

http://geometrictools.com/
&
http://cmldev.net/ [The CML (Configurable Math Library) is a free C++ math library for games and graphics.]
&
http://tog.acm.org/resources/GraphicsGems/gems.html
&
http://www.flipcode.com/documents/matrfaq.html
« Last Edit: September 02, 2011, 10:53:56 pm by DeVo »

kaefer

  • Swamp Rat
  • Posts: 572
Re: Euler angles from transformation matrix?
« Reply #3 on: September 05, 2011, 11:48:38 am »
http://www.flipcode.com/documents/matrfaq.html

Many thanks! Those links sure do help:
Code: [Select]
    // Implementation lifted from
    // http://www.flipcode.com/documents/matrfaq.html#Q37
    let getAngles (mat: Matrix3d) =
        let mat =  (Scale3d.RemoveScale mat).Inverse().GetMatrix() * mat
        let ay = acos(mat.ElementAt(0, 2)) - System.Math.PI / 2.
        let c = cos ay
        let (ax, az) =
            if abs c > 0.005 then
                let xtrx = mat.ElementAt(2, 2) / c
                let xtry = -mat.ElementAt(1, 2) / c
                let ztrx = mat.ElementAt(0, 0) / c
                let ztry = -mat.ElementAt(0, 1) / c
                atan2 xtry xtrx, atan2 ztry ztrx
            else
                let ztrx = mat.ElementAt(1, 1)
                let ztry = -mat.ElementAt(1, 0)
                0., atan2 ztry ztrx
        // if you want to get the result in degrees
        let toDegrees = 180. / System.Math.PI
        ax * toDegrees,
        ay * toDegrees,
        az * toDegrees

LE3

  • Guest
Re: Euler angles from transformation matrix?
« Reply #4 on: September 05, 2011, 11:55:12 am »
^
Great.
« Last Edit: September 05, 2011, 12:16:57 pm by DeVo »

gile

  • Water Moccasin
  • Posts: 2044
  • Marseille, France
Re: Euler angles from transformation matrix?
« Reply #5 on: December 18, 2016, 10:48:34 am »
Hi,

I know this is an old post but a similar question in the Autodesk Discussion group and I wanted to share here my reply.
It implements a base 'EulerAngles' for common purposes and two classes for proper Euler angles and Tait-Bryan angles which allows conversions from Matrix3d to angles and vice-versa.

A base 'EulerAngles' abstract class
Code - C#: [Select]
  1. using Autodesk.AutoCAD.Geometry;
  2. using static System.Math;
  3.  
  4. namespace Gile.AutoCAD.Geometry
  5. {
  6.    /// <summary>
  7.    /// Base class for Euler angles defintions.
  8.    /// </summary>
  9.    public abstract class EulerAngles
  10.    {
  11.        protected double psi, theta, phi;
  12.        protected Matrix3d xform;
  13.  
  14.        /// <summary>
  15.        /// Gets the angle alpha (or psi).
  16.        /// </summary>
  17.        public double Alpha => psi;
  18.  
  19.        /// <summary>
  20.        /// Gets the angle beta (or theta).
  21.        /// </summary>
  22.        public double Beta => theta;
  23.  
  24.        /// <summary>
  25.        /// Gets the angle gamma (or phi).
  26.        /// </summary>
  27.        public double Gamma => phi;
  28.  
  29.        /// <summary>
  30.        /// Get the transformation matrix.
  31.        /// </summary>
  32.        public Matrix3d Transform => xform;
  33.  
  34.        /// <summary>
  35.        /// Base constructor.
  36.        /// </summary>
  37.        /// <param name="transform">Transformation matrix.</param>
  38.        public EulerAngles(Matrix3d transform)
  39.        {
  40.            if (!transform.IsUniscaledOrtho())
  41.                throw new System.ArgumentException("Non uniscaled ortho matrix.");
  42.            xform = transform;
  43.        }
  44.  
  45.        /// <summary>
  46.        /// Base constructor.
  47.        /// </summary>
  48.        /// <param name="alpha">Precession angle.</param>
  49.        /// <param name="beta">Nutation angle.</param>
  50.        /// <param name="gamma">Intrinsic rotation.</param>
  51.        public EulerAngles(double alpha, double beta, double gamma)
  52.        {
  53.            psi = Wrap(alpha);
  54.            theta = Wrap(beta);
  55.            phi = Wrap(gamma);
  56.        }
  57.  
  58.        /// <summary>
  59.        /// Equality operator.
  60.        /// </summary>
  61.        /// <param name="x">Left operand.</param>
  62.        /// <param name="y">Right operand.</param>
  63.        /// <returns>true if the values of its operands are equal, false otherwise.</returns>
  64.        public static bool operator ==(EulerAngles x, EulerAngles y) => x.Equals(y);
  65.  
  66.        /// <summary>
  67.        /// Inequality operator.
  68.        /// </summary>
  69.        /// <param name="x">Left operand.</param>
  70.        /// <param name="y">Right operand.</param>
  71.        /// <returns>true if the values of its operands are not equal, false otherwise.</returns>
  72.        public static bool operator !=(EulerAngles x, EulerAngles y) => !x.Equals(y);
  73.  
  74.        /// <summary>
  75.        /// Determines whether the specified object is equal to the current object.
  76.        /// </summary>
  77.        /// <param name="obj">The object to compare with the current object.</param>
  78.        /// <returns>true if the specified object is equal to the current object; otherwise, false.</returns>
  79.        public override bool Equals(object obj)
  80.        {
  81.            var other = obj as EulerAngles;
  82.            return psi == other?.psi && theta == other.theta && phi == other.phi;
  83.        }
  84.  
  85.        /// <summary>
  86.        /// Serves as hash function.
  87.        /// </summary>
  88.        /// <returns>A hash code for the current object.</returns>
  89.        public override int GetHashCode() => psi.GetHashCode() ^ theta.GetHashCode() ^ phi.GetHashCode();
  90.  
  91.        /// <summary>
  92.        /// Returns a string that represents the current object.
  93.        /// </summary>
  94.        /// <returns>A string that represents the current object.</returns>
  95.        public override string ToString() => $"({Alpha}, {Beta}, {Gamma})";
  96.  
  97.        /// <summary>
  98.        /// Return the angle in [0,2*PI) range.
  99.        /// </summary>
  100.        private double Wrap(double angle) =>
  101.            0.0 <= angle && angle < 2 * PI ? angle : Asin(Sin(angle));
  102.    }
  103. }

The 'ProperEuler' class (can also be used with Normal and Rotation)
Transform = Rotation_Z(Alpha) * Rotation_X(Beta) * Rotation_Z(Gamma)
Code - C#: [Select]
  1. using Autodesk.AutoCAD.Geometry;
  2. using static System.Math;
  3.  
  4. namespace Gile.AutoCAD.Geometry
  5. {
  6.    /// <summary>
  7.    /// Defines the relations between a transformation matrix and Euler angles
  8.    /// (proper Euler angles using z-x'-z" convention).
  9.    /// </summary>
  10.    public class ProperEuler : EulerAngles
  11.    {
  12.        Matrix3d planeToWorld;
  13.  
  14.        /// <summary>
  15.        /// Gets the normal of the plane.
  16.        /// </summary>
  17.        public Vector3d Normal => planeToWorld.CoordinateSystem3d.Zaxis;
  18.  
  19.        /// <summary>
  20.        /// Gets the rotation on the plane;
  21.        /// </summary>
  22.        public double Rotation => phi;
  23.  
  24.        /// <summary>
  25.        /// Create a new intance of ProperEuler.
  26.        /// </summary>
  27.        /// <param name="transform">Transformation matrix.</param>
  28.        public ProperEuler(Matrix3d transform) : base(transform)
  29.        {
  30.            theta = Acos(xform[2, 2] / xform.GetScale());
  31.            if (Abs(theta) < 1e-7)
  32.            {
  33.                theta = 0.0;
  34.                psi = Atan2(xform[1, 0], xform[1, 1]);
  35.                phi = 0.0;
  36.            }
  37.            else
  38.            {
  39.                psi = Atan2(xform[0, 2], -xform[1, 2]);
  40.                phi = Atan2(xform[2, 0], xform[2, 1]);
  41.            }
  42.            planeToWorld =
  43.                Matrix3d.Rotation(psi, Vector3d.ZAxis, Point3d.Origin) *
  44.                Matrix3d.Rotation(theta, Vector3d.XAxis, Point3d.Origin);
  45.        }
  46.  
  47.        /// <summary>
  48.        /// Create a new intance of ProperEuler.
  49.        /// </summary>
  50.        /// <param name="alpha">Rotation angle around the Z axis.</param>
  51.        /// <param name="beta">Rotation angle around the X' axis.</param>
  52.        /// <param name="gamma">Rotation angle around the Z" axis.</param>
  53.        public ProperEuler(double alpha, double beta, double gamma) : base(alpha, beta, gamma)
  54.        {
  55.            planeToWorld =
  56.                Matrix3d.Rotation(alpha, Vector3d.ZAxis, Point3d.Origin) *
  57.                Matrix3d.Rotation(beta, Vector3d.XAxis, Point3d.Origin);
  58.            xform = planeToWorld *
  59.                Matrix3d.Rotation(gamma, Vector3d.ZAxis, Point3d.Origin);
  60.        }
  61.  
  62.        /// <summary>
  63.        /// Create a new intance of ProperEuler.
  64.        /// </summary>
  65.        /// <param name="normal">Plane normal.</param>
  66.        /// <param name="rotation">Proper rotation.</param>
  67.        public ProperEuler(Vector3d normal, double rotation)
  68.            : this(Matrix3d.PlaneToWorld(normal) *
  69.                   Matrix3d.Rotation(rotation, Vector3d.ZAxis, Point3d.Origin))
  70.        { }
  71.  
  72.    /// <summary>
  73.    /// Determines whether the specified object is equal to the current object.
  74.    /// </summary>
  75.    /// <param name="obj">The object to compare with the current object.</param>
  76.    /// <returns>true if the specified object is equal to the current object; otherwise, false.</returns>
  77.    public override bool Equals(object obj) => obj is ProperEuler && base.Equals(obj);
  78.  
  79.        /// <summary>
  80.        /// Serves as hash function.
  81.        /// </summary>
  82.        /// <returns>A hash code for the current object.</returns>
  83.        public override int GetHashCode() => base.GetHashCode();
  84.    }
  85. }

The TaitBryan class (yaw,pitch, roll)
Transform = Rotation_Z(Alpha) * Rotation_Y(Beta) * Rotation_X(Gamma)
Code - C#: [Select]
  1. using Autodesk.AutoCAD.Geometry;
  2. using static System.Math;
  3.  
  4. namespace Gile.AutoCAD.Geometry
  5. {
  6.    /// <summary>
  7.    /// Defines the relations between a transformation matrix and Euler angles
  8.    /// (specific Euler angles called Tait-Ryan angles using z-y'-x" convention).
  9.    /// </summary>
  10.    public class TaitBryan : EulerAngles
  11.    {
  12.        /// <summary>
  13.        /// Create a new intance of TaitBryan.
  14.        /// </summary>
  15.        /// <param name="transform">Transformation matrix.</param>
  16.        public TaitBryan(Matrix3d transform) : base(transform)
  17.        {
  18.            theta = -Asin(xform[2, 0] / xform.GetScale());
  19.            if (Abs(theta - PI * 0.5) < 1e-7)
  20.            {
  21.                theta = PI * 0.5;
  22.                psi = Atan2(xform[1, 2], xform[1, 1]);
  23.                phi = 0.0;
  24.            }
  25.            else if (Abs(Beta + PI * 0.5) < 1e-7)
  26.            {
  27.                theta = -PI * 0.5;
  28.                psi = Atan2(-xform[1, 2], xform[1, 1]);
  29.                phi = 0.0;
  30.            }
  31.            else
  32.            {
  33.                psi = Atan2(xform[1, 0], xform[0, 0]);
  34.                phi = Atan2(xform[2, 1], xform[2, 2]);
  35.            }
  36.        }
  37.  
  38.        /// <summary>
  39.        /// Create a new intance of TaitBryan.
  40.        /// </summary>
  41.        /// <param name="alpha">Rotation angle around the Z axis.</param>
  42.        /// <param name="beta">Rotation angle around the Y' axis.</param>
  43.        /// <param name="gamma">Rotation angle around the X" axis.</param>
  44.        public TaitBryan(double alpha, double beta, double gamma) : base(alpha, beta, gamma)
  45.        {
  46.            xform =
  47.                Matrix3d.Rotation(alpha, Vector3d.ZAxis, Point3d.Origin) *
  48.                Matrix3d.Rotation(beta, Vector3d.YAxis, Point3d.Origin) *
  49.                Matrix3d.Rotation(gamma, Vector3d.XAxis, Point3d.Origin);
  50.        }
  51.  
  52.        /// <summary>
  53.        /// Determines whether the specified object is equal to the current object.
  54.        /// </summary>
  55.        /// <param name="obj">The object to compare with the current object.</param>
  56.        /// <returns>true if the specified object is equal to the current object; otherwise, false.</returns>
  57.        public override bool Equals(object obj) => obj is TaitBryan && base.Equals(obj);
  58.  
  59.        /// <summary>
  60.        /// Serves as hash function.
  61.        /// </summary>
  62.        /// <returns>A hash code for the current object.</returns>
  63.        public override int GetHashCode() => base.GetHashCode();
  64.    }
  65. }
« Last Edit: December 18, 2016, 06:43:32 pm by gile »
Speaking English as a French Frog