Anytime you use trigonometry to solve a 3D problem, you're probably doing it wrong.
The ONLY time trigonometry is the right solution, is when you start with a measurement in degrees. This is the case when you have user input from a mouse, or when you have animation keyframes that use degrees (or radians.)
In the current case, there's a much easier way to recover the orientation matrix: Simply use the points as your basis vectors.
Let's say that the staff is modeled such that the “base to tip” direction is the X axis in the original model space.
Then, the X vector of your world orientation matrix is simply Xout = normalize(B - A)
The next step is to get the “up” vector. Assume this is “Y” in the model's original space. Use the world's up vector for the desired target, unless you have a full orientation value for the points A. (If you have the full orientation of point A, use its up vector)
So, the “up” vector (Y
in this case) is the world-up, made orthogonal to your “forward” vector. Let's say your world uses “Z up." Thus, the Y vector of your orientation matrix is Yout = fabsf(dot(Xout, Z)) > 0.999f ? Forward : normalize(Z - dot(Xout, Z) * Xout)
What's with the conditional? Well, when the user points the staff straight down, or straight up, there's no well-defined separate “up” because the staff-forward is also world-up. We pick an arbitrary axis at this point; I'm choosing Forward (which should be the user's forward vector) but you can pick whatever. It will approximately neve rhappen, but when it does, you don't want divisions by zero and NaNs in your matrices.
OK, we have “forward” and “up,” and they are orthonormal, so all we need to do is recover “right” using cross product:
Zout = cross(Xout, Yout)
Your rotation matrix then uses these vectors as its basis vector:
OutRotation = Matrix3(Xout, Yout, Zout)
Note that this is quite similar to the “lookat” matrix, except it's forward instead of inverse. Thus, you could just use whatever lookat
function you have, to look at B from A, and then transpose the matrix.