How to solve for the angle of a axis/angle rotation that gets me closest to a specific orientation

Question

I’m using a 3D affine transform (a 4x4 matrix of floats) to represent a combination of translations and rotations in 3D space doing a sort of 3D turtle graphics kind of thing. From this state I know the heading that I’m going, and what I’d like to do is roll around this particular axis. The problem that I’m trying to solve is determining the angle (let’s call it θ) to roll that results in one of the heading vectors being as close to vertical (+Y) as possible while still on the plane of rotation that’s constrained to the “heading” axis.

My starting points for the “forward” heading is +1 on the Y axis, and a local “up” heading as +1 on the Z axis.

The way I was trying to tackle this problem was applying the built-up set of translations and rotations using the 3D Affine transform and then figuring out if there was trigonometry I could use to solve for the angle. Since I know the axis around which I want to roll (or can compute it by applying the transform to the unit vector that represents it - the (0,0,1) vector), I was looking at the Rodrigues rotational formula, but my linear algebra (matrix math) skills and understanding are fairly weak - and I couldn’t see a path to solving that equation for θ given known vectors for the heading, or vectors on the plane that can be used to define a cross-product that is the heading, as well as knowing the world vector.

I’m heading towards trying to solve this by iteratively applying various values of θ and homing in on the solution based on the the resulting value that has the Y component value. I can apply the roll as an affine transform that I multiply onto the current transform, and then test the result of a unit “up” vector - rinse and repeat to find the one that gives me the best “Y” component value.

I’d like to know If there’s a way to solve this directly - to compute the value of θ that I can use to directly do the rotation, rather than numerically iterate/solve into the solution.

If the background of "why" makes any difference, I'm implementing a 3D rendering of a Lindenmayer System similar to what's in The Algorithmic Beauty of Plants, and there's a specific roll rendering command that I'm trying to sort out: '$', which rolls about the axis of the current heading, so that the "up" vector is vertical.

Answer 1

Here's a similar approach that you may find has better numerical stability / avoids the NaNs you're getting occasionally. I'm assuming a left-handed coordinate system since that's what I use most often, but you can flip it to a right-handed form easily enough if needed.

planeRight = normalize(cross(worldUp, heading));

planeUp = cross(heading, planeRight);

These two vectors form a basis for the plane perpendicular to your heading.

(Obviously this becomes indeterminate when your heading is pointing exactly up/down - best to pause your rolling then, or gradually fade-out the roll as the heading gets extremely steep)

We can then get the angle from your current up vector to the one in this plane that best aligns with worldUp as...

angle = atan2(dot(currentUp, planeRight), dot(currentUp, planeUp));

You can think of the dot products as getting us the x & y coordinates of our current up vector in this plane, and from that the two-argument arctangent gets us the angle of the vector from the positive x-axis in that plane. Here I've arranged the basis vectors so that we get an angle of 0 when we're already in perfect alignment, a negative angle when we need to roll in the negative direction, and a positive angle when we need to roll in the positive direction.

Answer 2

I got an answer from a local friend who saw these, and I've mostly implemented it. There's a few numerical instability points I need to sort out, but the gist is: Yes, there's definitely a way to explicitly calculate the angle of rotation needed.

The flow of the process is as follows:

• start with a unit vector that represents the 'up' vector that I want to compare (+Z in my case)
• pull the 3x3 translation matrix from the affine matrix, and use that to rotate the 'up' vector. We'll be comparing against this later to get the angle. Because it's explicitly set as a unit vector in the 'up' direction, we already know it's on the plane that can be represented by the normal of that plane - which is our heading vector.
• Use a similar technique to rotate the 'heading' vector (what starts as +Y for me) by the rotation matrix.

(a quick double check here I did in my testing was that these two remained 90° apart after rotation - primarily to verify that I didn't screw up the rotation calculation)

• Now that we have the heading, we use that as a normal vector for the plane upon which we want to project our two vectors, and from which we can get the angle desired. The vector (the 'rotated up' vector) is already on this plane. The other vector is the +Y world vector - the "as vertical as possible" component of this.

The formula for projecting a vector on a plane is:

vec_projected = vector - ( ( vector • plane_normal ) / plane_normal.length^2 ) * plane_normal

You can look at this conceptually as taking the vector you want to project and subtracting from it the portion of the vector that corresponds to the normal vector, which leaves you with just the component that's aligned on the plane.

🎩-tip to Greg Titus, who referred me to ProjectionOfVectorOntoPlan at MapleSoft.

• Once both the vectors are projected onto that base plane, then you can use the equation to calculate the angle between two vectors:

acos(
    dot(rotated_up_vector, projected_vector) /
            ( length(projected_vector) * length(rotated_up_vector) 
    )
 )

There's some numerical instability points I need to work out where my current code is returning 'NaN' from the final comparison, and the directional component isn't included in that - so I need to sort out a way to determine which direction to rotate, but the fundamentals part of it is at least sorted.