How to Calculate the Angle from Rotation Matrix

How to calculate the angle from rotation matrix

We can get Euler angles from rotation matrix using following formula.

Given a 3×3 rotation matrix

The 3 Euler angles are

Here atan2 is the same arc tangent function, with quadrant checking, you typically find in C or Matlab.

Note: Care must be taken if the angle around the y-axis is exactly +/-90°. In that case all elements in the first column and last row, except the one in the lower corner, which is either 1 or -1, will be 0 (cos(1)=0). One solution would be to fix the rotation around the x-axis at 180° and compute the angle around the z-axis from: atan2(r_12, -r_22).

See also https://www.geometrictools.com/Documentation/EulerAngles.pdf, which includes implementations for six different orders of Euler angles.

How to calculate the rotation angles needed on X and Z to align two vectors

It's rather mathematical question I am not sure how to write math properly here but you can do following. If you rotate first around X-axis and then around Z-axis your last rotation do not change z projection. If (a, b, c) is the starting normed vector and (x, y, z) is the end normed vector you can write b * sin(f) + c * cos(f) = z based on rotation matrix around X-axis, where f is rotation angle around X-axis. Then based on equality from wikipedia (it seems not quite right: sng(c) part should be dropped) you can find the value of f. So you can calculate X-axis rotation matrix and get the vector after applying this rotation (a', b', c'). Then multiplying it to Z-axis rotation matrix and writing equalities for x and y you can find the values of sin and cos of Z-axis rotation angle.

import numpy as np

vector1 = np.array([0., -1., 0.])
vector2 = np.array([0.2, -0.2, -0.5])
vector2 = vector2 / np.linalg.norm(vector2)
a, b, c = vector1
x, y, z = vector2
def angle(b, c, z):
    return np.arccos(z / np.sqrt(b ** 2 + c ** 2)) - np.arctan2(-b, c)

x_angle = angle(b, c, z)
x_after_x_rotation = a
y_after_x_rotation = b * np.cos(x_angle) - c * np.sin(x_angle)

det = np.sqrt(x_after_x_rotation ** 2 + y_after_x_rotation ** 2)
sin = x_after_x_rotation * y - y_after_x_rotation * x
cos = y_after_x_rotation * y + x_after_x_rotation * x
sin /= det
cos /= det
z_angle = np.arctan2(sin, cos)

print(np.rad2deg(x_angle), np.rad2deg(z_angle))
# 60.50379150343357 45.0

Determining the angle for rotation matrix from axis and angle

There is a well-known identity linking the cross-product of two vectors to the angle between them:

Where theta is the smaller angle. However, this can be in the range [0, 180], over which the inverse sine function is multi-valued: an acute angle theta is such that sin(theta) = sin(180 - theta), so we can't directly obtain it from this formula.

We can use the dot-product instead:

The inverse cosine function is single-valued over this range, so we can use it!

dot = Dot(Vector1, Vector2)
cos = dot / (Len(Vector1) * Len(Vector2))
theta_radians = acos(cos)
theta_degrees = theta_radians * 180 / PI

Calculate angle from matrix transform

You can use this:

var x = new Vector(1, 0);
Vector rotated = Vector.Multiply(x, matrix);
double angleBetween = Vector.AngleBetween(x, rotated);

The idea is:

1. We create a tempvector (1,0)
2. We apply the matrix transform on the vector and get a rotated temp vector
3. We calculate the angle between the original and the rotated temp vector

You can play around with this:

[TestCase(0,0)]
[TestCase(90,90)]
[TestCase(180,180)]
[TestCase(270,-90)]
[TestCase(-90, -90)]
public void GetAngleTest(int angle, int expected)
{
    var matrix = new RotateTransform(angle).Value;
    var x = new Vector(1, 0);
    Vector rotated = Vector.Multiply(x, matrix);
    double angleBetween = Vector.AngleBetween(x, rotated);
    Assert.AreEqual(expected,(int)angleBetween);
}

Given start point, angles in each rotational axis and a direction, calculate end point

Based in the three angles you have to construct the 3x3 rotation matrix. Then each column of the matrix represents the local x, y and z directions. If you have a local direction you want to move by, then multiply the 3x3 rotation with the direction vector to get the result in global coordinates.

I made a little intro to 3D coordinate transformations that I think will answer your question.

3D Coordinates

Finding Angle from transform matrix

If it's only about the rotation, one can transform the vector (1,0) using the given matrix, and compute the angle between the resulting vector and the x-axis, as already mentioned in the comment to the original question

import java.awt.Point;
import java.awt.geom.AffineTransform;
import java.awt.geom.Point2D;
import java.util.Random;

public class ExtractRotation
{
    public static void main(String[] args)
    {
        for (int i=0; i<=180; i++)
        {
            double angleRad = Math.toRadians(i);
            AffineTransform at = createRandomTransform(angleRad);
            double extractedAngleRad = extractAngle(at);
            System.out.println(
                "In: "+Math.toDegrees(angleRad)+ " " +
                "Out "+Math.toDegrees(extractedAngleRad));
        }
    }

    private static double extractAngle(double m[])
    {
        return extractAngle(new AffineTransform(m));
    }
    private static double extractAngle(AffineTransform at)
    {
        Point2D p0 = new Point();
        Point2D p1 = new Point(1,0);
        Point2D pp0 = at.transform(p0, null);
        Point2D pp1 = at.transform(p1, null);
        double dx = pp1.getX() - pp0.getX();
        double dy = pp1.getY() - pp0.getY();
        double angle = Math.atan2(dy, dx);
        return angle;
    }

    private static Random random = new Random(0); 
    private static AffineTransform createRandomTransform(double angleRad)
    {
        AffineTransform at = new AffineTransform();
        double scale = 1.0;
        at.translate(randomDouble(), randomDouble());
        scale = Math.abs(randomDouble());
        at.scale(scale, scale);
        at.rotate(angleRad);
        at.translate(randomDouble(), randomDouble());
        scale = Math.abs(randomDouble());
        at.scale(scale, scale);
        return at;
    }

    private static double randomDouble()
    {
        return -5.0 + random.nextDouble() * 10;
    }

}

---

Related Topics